Let's get Kraken

Author: ohowe1

src/main/java/frc/robot/Autos.java

@@ -20,7 +20,6 @@
 import frc.robot.subsystems.swerve.SwerveDriveSubsystem;
 import frc.robot.subsystems.transport.TransportSubsystem;
 import frc.robot.subsystems.wrist.WristSubsystem;
-import frc.robot.utils.RaiderUtils;
 
 public class Autos {
   private final SwerveDriveSubsystem driveSubsystem;
@@ -133,11 +132,13 @@ public Command ampSpeakerCloseAmpTwoPieceAuto() {
   }
 
   public Command mobilitySourceSideAuto() {
-    return Commands.sequence(autoStart(), followPathCommand("MobilitySourceSide", true, driveSubsystem));
+    return Commands.sequence(
+        autoStart(), followPathCommand("MobilitySourceSide", true, driveSubsystem));
   }
 
   public Command mobilityAmpSideAuto() {
-    return Commands.sequence(autoStart(), followPathCommand("MobilityAmpSide", true, driveSubsystem));
+    return Commands.sequence(
+        autoStart(), followPathCommand("MobilityAmpSide", true, driveSubsystem));
   }
 
   private Command centerSpeakerCloseMidNote(boolean firstPath) {

src/main/java/frc/robot/Constants.java

@@ -114,18 +114,27 @@ private SwerveConstants() {}
     public static final double STEER_GEAR_REDUCTION = 150.0 / 7.0;
 
     public static final double DRIVE_CURRENT_LIMIT = 60.0;
-    public static final int STEER_STALL_CURRENT_LIMIT = 45;
-    public static final int STEER_FREE_CURRENT_LIMIT = 25;
+    public static final double STEER_CURRENT_LIMIT = 35.0;
 
     // 0.47
     public static final TunablePIDGains DRIVE_VELOCITY_PID_GAINS =
         new TunablePIDGains("/gains/drive", 0.2, 0.0, 0.0, MiscConstants.TUNING_MODE);
-
+    // Meters per rotation
+    private static final double DRIVE_CONVERSION =
+        (Math.PI * WHEEL_DIAMETER_METERS) / DRIVE_GEAR_REDUCTION;
+    // Volts/(Meter/Second) to Volts/(Rotation/Second)
     public static final TunableFFGains DRIVE_VELOCITY_FF_GAINS =
-        new TunableFFGains("/gains/drive", 0.064022, 1.9843, 0.9255, MiscConstants.TUNING_MODE);
+        new TunableFFGains(
+            "/gains/drive",
+            0.064022 * DRIVE_CONVERSION,
+            1.9843 * DRIVE_CONVERSION,
+            0.9255 * DRIVE_CONVERSION,
+            MiscConstants.TUNING_MODE);
 
     public static final TunablePIDGains STEER_POSITION_PID_GAINS =
-        new TunablePIDGains("/gains/steer", 1.0, 0.0, 0.1, MiscConstants.TUNING_MODE);
+        new TunablePIDGains("/gains/steer", 7.207, 0.0, 0.002, MiscConstants.TUNING_MODE);
+    public static final TunableFFGains STEER_VELOCITY_FF_GAINS =
+        new TunableFFGains("/gains/steer", 0.0, 0.0, 0.0, MiscConstants.TUNING_MODE);
 
     // Left right distance between center of wheels
     public static final double TRACKWIDTH_METERS = Units.inchesToMeters(22.75);
@@ -154,9 +163,9 @@ private SwerveConstants() {}
             DRIVE_GEAR_REDUCTION,
             STEER_GEAR_REDUCTION,
             DRIVE_CURRENT_LIMIT,
-            STEER_FREE_CURRENT_LIMIT,
-            STEER_STALL_CURRENT_LIMIT,
+            STEER_CURRENT_LIMIT,
             WHEEL_DIAMETER_METERS,
+            MAX_VELOCITY_METERS_SECOND,
             ODOMETRY_FREQUENCY,
             DRIVE_VELOCITY_PID_GAINS,
             DRIVE_VELOCITY_FF_GAINS,

src/main/java/frc/robot/RobotContainer.java

@@ -93,8 +93,7 @@ private void configureAutos() {
     autoCommand.addOption(
         "Center Speaker Two Piece Close Mid", autos.centerSpeakerCloseMidTwoPieceAuto());
     autoCommand.addOption(
-        "Center Speaker Three Piece Close Amp Mid",
-        autos.centerSpeakerCloseAmpMidThreePieceAuto());
+        "Center Speaker Three Piece Close Amp Mid", autos.centerSpeakerCloseAmpMidThreePieceAuto());
     autoCommand.addOption(
         "Center Speaker Three Piece Close Source Mid",
         autos.centerSpeakerCloseSourceMidThreePieceAuto());
@@ -230,9 +229,7 @@ private void configureDriving() {
     TunableDouble maxMaxAngularSpeedPercent = new TunableDouble("/speed/maxAngular", 0.6, true);
 
     DoubleSupplier maxTranslationalSpeedSuppler =
-        () ->
-            maxTranslationSpeedPercent.get()
-                * SwerveConstants.MAX_VELOCITY_METERS_SECOND;
+        () -> maxTranslationSpeedPercent.get() * SwerveConstants.MAX_VELOCITY_METERS_SECOND;
     DoubleSupplier maxAngularSpeedSupplier =
         () -> maxMaxAngularSpeedPercent.get() * SwerveConstants.MAX_ANGULAR_VELOCITY_RADIANS_SECOND;
 

src/main/java/frc/robot/commands/drive/auto/FollowPathCommand.java

@@ -41,12 +41,16 @@ public class FollowPathCommand extends Command {
   private final Timer timer = new Timer();
   private ChoreoTrajectory currentPath;
 
-  public FollowPathCommand(ChoreoTrajectory path, boolean resetOdometry, SwerveDriveSubsystem driveSubsystem) {
+  public FollowPathCommand(
+      ChoreoTrajectory path, boolean resetOdometry, SwerveDriveSubsystem driveSubsystem) {
     this(path, resetOdometry, true, driveSubsystem);
   }
 
   public FollowPathCommand(
-      ChoreoTrajectory path, boolean resetOdometry, boolean shouldFlipIfRed, SwerveDriveSubsystem driveSubsystem) {
+      ChoreoTrajectory path,
+      boolean resetOdometry,
+      boolean shouldFlipIfRed,
+      SwerveDriveSubsystem driveSubsystem) {
     this(
         () -> {
           if (shouldFlipIfRed && (RaiderUtils.shouldFlip())) {
@@ -59,7 +63,9 @@ public FollowPathCommand(
   }
 
   public FollowPathCommand(
-      Supplier<ChoreoTrajectory> pathSupplier, boolean resetOdometry, SwerveDriveSubsystem driveSubsystem) {
+      Supplier<ChoreoTrajectory> pathSupplier,
+      boolean resetOdometry,
+      SwerveDriveSubsystem driveSubsystem) {
     this.pathSupplier = pathSupplier;
     this.resetOdometry = resetOdometry;
     this.driveSubsystem = driveSubsystem;

src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java

@@ -19,7 +19,10 @@
 public class ClimberSubsystem extends SubsystemBase {
   private final TelemetryTalonFX climberMotor =
       new TelemetryTalonFX(
-          CLIMBER_MOTOR_ID, "/climber/motor", MiscConstants.CANIVORE_NAME, MiscConstants.TUNING_MODE);
+          CLIMBER_MOTOR_ID,
+          "/climber/motor",
+          MiscConstants.CANIVORE_NAME,
+          MiscConstants.TUNING_MODE);
   private final EventTelemetryEntry eventEntry = new EventTelemetryEntry("/climber/events");
   private final DoubleTelemetryEntry voltageReqEntry =
       new DoubleTelemetryEntry("/climber/voltageReq", MiscConstants.TUNING_MODE);

src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java

@@ -98,7 +98,8 @@ enum DriveMode {
   private final Function<Pose2d, List<EstimatedRobotPose>> cameraPoseDataSupplier;
 
   private final TelemetryPigeon2 pigeon =
-      new TelemetryPigeon2(PIGEON_ID, "/drive/gyro", MiscConstants.CANIVORE_NAME, MiscConstants.TUNING_MODE);
+      new TelemetryPigeon2(
+          PIGEON_ID, "/drive/gyro", MiscConstants.CANIVORE_NAME, MiscConstants.TUNING_MODE);
 
   private StatusSignal<Double> yawSignal;
 
@@ -113,11 +114,15 @@ enum DriveMode {
               null,
               this));
 
+  private final SysIdRoutine steerPositionSysId =
+      new SysIdRoutine(
+          new SysIdRoutine.Config(
+              null, null, null, (state) -> SignalLogger.writeString("State", state.toString())),
+          new SysIdRoutine.Mechanism(
+              (Measure<Voltage> voltage) -> setRawVolts(0.0, voltage.in(Volts)), null, this));
+
   private final Alert pigeonConfigurationAlert =
       new Alert("Pigeon failed to initialize", Alert.AlertType.ERROR);
-  private final BooleanTelemetryEntry allModulesAtAbsoluteZeroEntry =
-      new BooleanTelemetryEntry("/drive/allModulesAtAbsoluteZero", true);
-
   private final DoubleTelemetryEntry gyroEntry =
       new DoubleTelemetryEntry("/drive/gyroRadians", true);
 
@@ -327,20 +332,6 @@ public double[] getActualDriveVoltages() {
     return voltages;
   }
 
-  public void setAllModulesToAbsolute() {
-    for (SwerveModule module : modules) {
-      module.resetSteerToAbsolute();
-    }
-  }
-
-  private boolean allModulesAtAbsolute() {
-    boolean allSet = true;
-    for (SwerveModule module : modules) {
-      allSet &= module.isSetToAbsolute();
-    }
-    return allSet;
-  }
-
   /**
    * Should only be used for characterization
    *
@@ -376,14 +367,22 @@ public SwerveModulePosition[] getModulePositions() {
     return actualPositions;
   }
 
-  public Command quasistaticSysIDCommand(SysIdRoutine.Direction direction) {
+  public Command driveQuasistaticSysIDCommand(SysIdRoutine.Direction direction) {
     return driveVelocitySysId.quasistatic(direction).beforeStarting(SignalLogger::start);
   }
 
-  public Command dynamicSysIDCommand(SysIdRoutine.Direction direction) {
+  public Command driveDynamicSysIDCommand(SysIdRoutine.Direction direction) {
     return driveVelocitySysId.dynamic(direction).beforeStarting(SignalLogger::start);
   }
 
+  public Command steerQuasistaticSysIDCommand(SysIdRoutine.Direction direction) {
+    return steerPositionSysId.quasistatic(direction).beforeStarting(SignalLogger::start);
+  }
+
+  public Command steerDynamicSysIDCommand(SysIdRoutine.Direction direction) {
+    return steerPositionSysId.dynamic(direction).beforeStarting(SignalLogger::start);
+  }
+
   @Override
   public void periodic() {
     switch (driveMode) {
@@ -401,7 +400,7 @@ public void periodic() {
       }
       case CHARACTERIZATION -> {
         for (SwerveModule module : modules) {
-          module.setCharacterizationVoltage(rawDriveVolts);
+          module.setDriveCharacterizationVoltage(rawDriveVolts);
         }
       }
     }
@@ -418,7 +417,6 @@ public void periodic() {
   }
 
   private void logValues() {
-    allModulesAtAbsoluteZeroEntry.append(allModulesAtAbsolute());
     gyroEntry.append(getGyroRotation().getRadians());
     chassisSpeedsEntry.append(getCurrentChassisSpeeds());
     actualSwerveStatesEntry.append(getActualStates());