Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

qawsedrftgyhujikolp

Browse Source
This commit is contained in:
aarav18
2022-03-18 23:07:53 -06:00
parent e1ef2f8aff
commit 46396a704f
5 changed files with 63 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+9 -9
View File
@@ -36,7 +36,7 @@ public final class Constants {
public static final double TICKS_PER_ROTATION_FX = 2048;
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 4.0;
public static final double ROTATION_SPEED = 3.0;
public static final double WIDTH = 23.5;
public static final double HEIGHT = 23.5;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
@@ -203,18 +203,18 @@ public final class Constants {
// PID Constants
public static final int SHOULDER_POSITION_SLOT_IDX = 0;
public static final int SHOULDER_VELOCITY_SLOT_IDX = 1;
public static final int SHOULDER_PID_LOOP_IDX = 1;
public static final int SHOULDER_VELOCITY_SLOT_IDX = 0;
public static final int SHOULDER_PID_LOOP_IDX = 0;
public static final int ELBOW_POSITION_SLOT_IDX = 0;
public static final int ELBOW_VELOCITY_SLOT_IDX = 1;
public static final int ELBOW_PID_LOOP_IDX = 1;
public static final int ELBOW_VELOCITY_SLOT_IDX = 0;
public static final int ELBOW_PID_LOOP_IDX = 0;
public static final Gains SHOULDER_POSITION_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2);
public static final Gains ELBOW_POSITION_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2);
public static final Gains SHOULDER_POSITION_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.2);
public static final Gains ELBOW_POSITION_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.2);
public static final Gains SHOULDER_VELOCITY_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2);
public static final Gains ELBOW_VELOCITY_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2);
public static final Gains SHOULDER_VELOCITY_GAINS = new Gains(0.2, 0.0, 5.0, 0.0, 0, 0.2);
public static final Gains ELBOW_VELOCITY_GAINS = new Gains(0.3, 0.005, 5.0, 0.0, 0, 1.0);
public static final int CLIMBER_TIMEOUT_MS = 50;
src/main/java/frc4388/robot/Robot.java
+9 -9
View File
@@ -143,15 +143,15 @@ public class Robot extends TimedRobot {
// SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition());
// SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition());
current =
m_robotContainer.m_robotBoomBoom.getCurrent() +
m_robotContainer.m_robotClimber.getCurrent() +
m_robotContainer.m_robotHood.getCurrent() +
m_robotContainer.m_robotIntake.getCurrent() +
m_robotContainer.m_robotExtender.getCurrent() +
m_robotContainer.m_robotSerializer.getCurrent() +
m_robotContainer.m_robotStorage.getCurrent() +
m_robotContainer.m_robotSwerveDrive.getCurrent();
m_robotContainer.m_robotTurret.getCurrent();
// m_robotContainer.m_robotBoomBoom.getCurrent() +
m_robotContainer.m_robotClimber.getCurrent(); //+
// m_robotContainer.m_robotHood.getCurrent() +
// m_robotContainer.m_robotIntake.getCurrent() +
// m_robotContainer.m_robotExtender.getCurrent() +
// m_robotContainer.m_robotSerializer.getCurrent() +
// m_robotContainer.m_robotStorage.getCurrent() +
// m_robotContainer.m_robotSwerveDrive.getCurrent();
// m_robotContainer.m_robotTurret.getCurrent();
SmartDashboard.putNumber("Total Robot Current Draw", current);
SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage());
SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent());
src/main/java/frc4388/robot/RobotContainer.java
+7 -7
View File
@@ -173,8 +173,8 @@ public class RobotContainer {
// IK command
m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(),
getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000,
getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
// Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand(
@@ -226,11 +226,11 @@ public class RobotContainer {
// if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
// }, m_robotHood));
m_robotClimber.setDefaultCommand(
// new RunCommand(() -> {
// if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); }
/*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(),
getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //}
// m_robotClimber.setDefaultCommand(
// // new RunCommand(() -> {
// // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); }
// /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(),
// getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //}
// }, m_robotClimber));
// m_robotTurret.setDefaultCommand(
src/main/java/frc4388/robot/subsystems/Climber.java
+30 -24
View File
@@ -54,8 +54,9 @@ public class Climber extends SubsystemBase {
m_elbow.configFactoryDefault();
m_elbow.setNeutralMode(NeutralMode.Brake);
setClimberPositionGains();
// setClimberPositionGains();
setClimberVelocityGains();
useVelocityGains();
tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()};
@@ -90,24 +91,24 @@ public class Climber extends SubsystemBase {
this.elbowSpeedLimiter = 1.0;
}
public void setClimberPositionGains() {
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
m_shoulder.config_kF(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_shoulder.config_kP(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_shoulder.config_kI(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_shoulder.config_kD(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
// public void setClimberPositionGains() {
// m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
// m_shoulder.config_kF(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_shoulder.config_kP(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_shoulder.config_kI(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_shoulder.config_kD(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
m_elbow.config_kF(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.config_kP(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.config_kI(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
}
// m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
// m_elbow.config_kF(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_elbow.config_kP(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_elbow.config_kI(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
// m_elbow.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
// }
public void usePositionGains() {
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
}
// public void usePositionGains() {
// m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
// m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
// }
public void setClimberVelocityGains() {
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
@@ -198,8 +199,8 @@ public class Climber extends SubsystemBase {
// // shoulderPosition += m_shoulderOffset;
// // elbowPosition += m_elbowOffset;
m_shoulder.set(TalonFXControlMode.Position, shoulderAngle);
m_elbow.set(TalonFXControlMode.Position, elbowAngle);
// m_shoulder.set(TalonFXControlMode.Position, shoulderAngle);
// m_elbow.set(TalonFXControlMode.Position, elbowAngle);
}
public void setJointSpeeds(double[] speeds) {
@@ -212,15 +213,18 @@ public class Climber extends SubsystemBase {
}
boolean movingPrev = false;
boolean moving;
public void controlWithInput(double xInput, double yInput) {
boolean moving = xInput != 0 && yInput != 0;
moving = xInput != 0 || yInput != 0;
if(movingPrev != moving) {
if(moving) {
useVelocityGains();
SmartDashboard.putString("Climber Gains", "Velocity");
} else {
usePositionGains();
// usePositionGains();
tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()};
SmartDashboard.putString("Climber Gains", "Position");
}
}
@@ -242,13 +246,15 @@ public class Climber extends SubsystemBase {
int pCount = 0;
@Override
public void periodic() {
SmartDashboard.putBoolean("Moving", moving);
SmartDashboard.putBoolean("MovingPrev", movingPrev);
SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition());
SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
// double[] jointAngles = getTargetJointAngles(tPoint, 0.d);
if(pCount % 1 == 0)
setJointAngles(tJointAngles);
// if(pCount % 1 == 0)
// setJointAngles(tJointAngles);
pCount ++;
// pCount ++;
// * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output).
double currentElbowPos = this.m_elbow.getSelectedSensorPosition();
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+8 -4
View File
@@ -16,6 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -116,7 +117,7 @@ public class SwerveDrive extends SubsystemBase {
Translation2d speed = new Translation2d(leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust);
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightX, rightY);
rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = speed.getX();
double ySpeedMetersPerSecond = speed.getY();
@@ -199,9 +200,12 @@ public class SwerveDrive extends SubsystemBase {
*
* @return Rotation2d object holding current gyro in radians
*/
public Rotation2d getRegGyro() {
double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
return new Rotation2d(regCur * Math.PI / 180);
public Rotation2d getRegGyro() {
// * test chassis regression
// double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
// * new robot regression
double regCur = 0.2507023948 + m_gyro.getRotation2d().getDegrees() * 0.999034743;
return new Rotation2d(Math.toRadians(regCur));
}
/**