fix(swerve): Test on actual robot.

src/main/java/frc/robot/odometry/OdometryConstants.java

@@ -6,5 +6,5 @@
 public class OdometryConstants {
 
   /** Gyroscope's CAN identifier. */
-  public static final CAN GYROSCOPE_CAN = new CAN(0); // TODO
+  public static final CAN GYROSCOPE_CAN = new CAN(7, "swerve"); // TODO
 }

src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPID.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.util.Units;
 import frc.lib.CAN;
 import frc.lib.ConfigApplier;
+import frc.robot.swerve.SwerveConstants.MK4iConstants;
 
 /** TalonFX steer motor controlled by an external PID controller. */
 public class SteerMotorIOTalonFXPID extends SteerMotorIOTalonFX {
@@ -36,31 +37,34 @@ public SteerMotorIOTalonFXPID(CAN steerMotorCAN, CAN azimuthEncoderCAN) {
   public void configure() {
     TalonFXConfiguration config = SwerveFactory.createSteerMotorConfig();
 
+    config.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING;
+
     ConfigApplier.applyTalonFXConfig(talonFX.getConfigurator(), config);
   }
 
   @Override
   public void setSetpoint(double positionRotations) {
+    positionFeedback.setSetpoint(positionRotations);
+
     if (positionFeedback.atSetpoint()) {
       talonFX.setControl(new CoastOut());
     } else {
-      talonFX.setControl(calculatePositionVoltage(positionRotations));
+      talonFX.setControl(calculatePositionVoltage());
     }
   }
 
   /**
    * Calculates the TalonFX's applied voltage for a position setpoint.
    *
-   * @param positionRotations the position setpoint.
    * @return the voltage to apply.
    */
-  private VoltageOut calculatePositionVoltage(double positionRotations) {
+  private VoltageOut calculatePositionVoltage() {
     double measuredPositionRotations =
         BaseStatusSignal.getLatencyCompensatedValue(
             this.positionRotations, this.velocityRotationsPerSecond);
 
     double positionFeedbackVolts =
-        positionFeedback.calculate(measuredPositionRotations, positionRotations);
+        positionFeedback.calculate(measuredPositionRotations);
 
     double positionFeedforwardVolts =
         Math.signum(positionFeedbackVolts) * this.positionFeedforwardVolts;

src/main/java/frc/robot/swerve/Swerve.java

@@ -37,6 +37,13 @@ private Swerve() {
             SwerveConstants.NORTH_EAST_MODULE_CONFIG.position(),
             SwerveConstants.SOUTH_EAST_MODULE_CONFIG.position(),
             SwerveConstants.SOUTH_WEST_MODULE_CONFIG.position());
+
+    // TODO this command is so the swerve fails safely
+    this.setDefaultCommand(Commands.run(() -> {
+      for (int i = 0; i < 4; i++) {
+        swerveModules[i].setSetpoint(swerveModules[i].getState(), false);
+      }
+    }, this));
   }
 
   /**

src/main/java/frc/robot/swerve/SwerveConstants.java

@@ -37,28 +37,28 @@ public static class MK4iConstants {
   /** Module configuration for the north west swerve module. */
   public static final SwerveModuleConfig NORTH_WEST_MODULE_CONFIG =
       new SwerveModuleConfig(
-          new SwerveModuleCAN(1, 2, 3, SWERVE_BUS),
+          new SwerveModuleCAN(2, 1, 3, SWERVE_BUS),
           new Translation2d(X_OFFSET, Y_OFFSET),
           Rotation2d.fromRotations(-0.179688));
 
   /** Module configuration for the north east swerve module. */
   public static final SwerveModuleConfig NORTH_EAST_MODULE_CONFIG =
       new SwerveModuleConfig(
-          new SwerveModuleCAN(4, 5, 6, SWERVE_BUS),
+          new SwerveModuleCAN(5, 4, 6, SWERVE_BUS),
           new Translation2d(X_OFFSET, -Y_OFFSET),
           Rotation2d.fromRotations(-0.951904));
 
   /** Module configuration for the south east swerve module. */
   public static final SwerveModuleConfig SOUTH_EAST_MODULE_CONFIG =
       new SwerveModuleConfig(
-          new SwerveModuleCAN(8, 9, 10, SWERVE_BUS),
+          new SwerveModuleCAN(9, 8, 10, SWERVE_BUS),
           new Translation2d(-X_OFFSET, -Y_OFFSET),
           Rotation2d.fromRotations(-0.774568));
 
   /** Module configuration for the south west swerve module. */
   public static final SwerveModuleConfig SOUTH_WEST_MODULE_CONFIG =
       new SwerveModuleConfig(
-          new SwerveModuleCAN(11, 12, 13, SWERVE_BUS),
+          new SwerveModuleCAN(12, 11, 13, SWERVE_BUS),
           new Translation2d(-X_OFFSET, Y_OFFSET),
           Rotation2d.fromRotations(-0.954346));
 

src/main/java/frc/robot/swerve/SwerveFactory.java

@@ -51,8 +51,8 @@ public static CANcoderConfiguration createAzimuthEncoderConfig(Rotation2d offset
    *
    * @return a steer motor.
    */
-  public static SteerMotorIO createSteerMotor() {
-    if (Robot.isReal()) return new SteerMotorIOSim();
+  public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) {
+    if (Robot.isReal()) return new SteerMotorIOTalonFXPID(config.moduleCAN().steer(), config.moduleCAN().azimuth());
 
     return new SteerMotorIOSim();
   }

src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java

@@ -37,7 +37,7 @@ public SwerveModuleIOCustom(SwerveModuleConfig config) {
     azimuthEncoder = SwerveFactory.createAzimuthEncoder(config);
     azimuthEncoder.configure();
 
-    steerMotor = SwerveFactory.createSteerMotor();
+    steerMotor = SwerveFactory.createSteerMotor(config);
     steerMotor.configure();
 
     driveMotor = SwerveFactory.createDriveMotor(config);
@@ -64,7 +64,7 @@ public void setSetpoint(SwerveModuleState setpoint, boolean lazy) {
    * @return the optimized setpoint.
    */
   private SwerveModuleState optimize(SwerveModuleState setpoint) {
-    return setpoint; // TODO
+    return setpoint;
   }
 
   @Override