feat: use fused CANCoder for swerve turn position

Author: mimizh2418

src/main/java/org/team1540/robot2024/subsystems/drive/Module.java

@@ -20,8 +20,6 @@ public class Module {
     private final PIDController turnFeedback;
     private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop
     private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop
-    private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute
-    private boolean forceTurn = false;
     private double lastPositionMeters = 0.0; // Used for delta calculation
 
     public Module(ModuleIO io, int index) {
@@ -55,17 +53,10 @@ public Module(ModuleIO io, int index) {
 
     public void periodic() {
         io.updateInputs(inputs);
-        Logger.processInputs("Drive/Module" + index, inputs);
-
-        // On first cycle, reset relative turn encoder
-        // Wait until absolute angle is nonzero in case it wasn't initialized yet
-        if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) {
-            turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition);
-        }
+        Logger.processInputs("Drivetrain/Module" + index, inputs);
 
         // Run closed loop turn control
         if (angleSetpoint != null) {
-
             io.setTurnVoltage(
                     turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians()));
 
@@ -143,11 +134,7 @@ public void setBrakeMode(boolean enabled) {
      * Returns the current turn angle of the module.
      */
     public Rotation2d getAngle() {
-        if (turnRelativeOffset == null) {
-            return new Rotation2d();
-        } else {
-            return inputs.turnPosition.plus(turnRelativeOffset);
-        }
+        return inputs.turnPosition;
     }
 
     /**

src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOSim.java

@@ -16,7 +16,6 @@
  * approximation for the behavior of the module.
  */
 public class ModuleIOSim implements ModuleIO {
-    private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI);
     private final DCMotorSim driveSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), DRIVE_GEAR_RATIO, 0.025);
     private final DCMotorSim turnSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), TURN_GEAR_RATIO, 0.004);
     private double driveAppliedVolts = 0.0;
@@ -32,7 +31,7 @@ public void updateInputs(ModuleIOInputs inputs) {
         inputs.driveAppliedVolts = driveAppliedVolts;
         inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps());
 
-        inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition);
+        inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad());
         inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad());
         inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec();
         inputs.turnAppliedVolts = turnAppliedVolts;

src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java

@@ -58,13 +58,13 @@ public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) {
         drivePosition = driveTalon.getPosition();
         driveVelocity = driveTalon.getVelocity();
         driveAppliedVolts = driveTalon.getMotorVoltage();
-        driveCurrent = driveTalon.getStatorCurrent();
+        driveCurrent = driveTalon.getSupplyCurrent();
 
         turnAbsolutePosition = cancoder.getAbsolutePosition();
         turnPosition = turnTalon.getPosition();
         turnVelocity = turnTalon.getVelocity();
         turnAppliedVolts = turnTalon.getMotorVoltage();
-        turnCurrent = turnTalon.getStatorCurrent();
+        turnCurrent = turnTalon.getSupplyCurrent();
 
         BaseStatusSignal.setUpdateFrequencyForAll(100.0, drivePosition, turnPosition); // Required for odometry, use faster rate
         BaseStatusSignal.setUpdateFrequencyForAll(
@@ -111,8 +111,8 @@ public void updateInputs(ModuleIOInputs inputs) {
         inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
 
         inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
-        inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO);
-        inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO;
+        inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble());
+        inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble());
         inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();
         inputs.turnCurrentAmps = turnCurrent.getValueAsDouble();
     }

src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java

@@ -5,6 +5,7 @@
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import org.team1540.robot2024.Constants;
@@ -49,6 +50,11 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
             if (canbus == null) {
                 canbus = "";
             }
+
+            int driveID = 30 + id;
+            int turnID = 20 + id;
+            int canCoderID = 10 + id;
+
             TalonFXConfiguration driveConfig = new TalonFXConfiguration();
             TalonFXConfiguration turnConfig = new TalonFXConfiguration();
             CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
@@ -62,21 +68,23 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
             turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0;
             turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
             turnConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+            turnConfig.Feedback.FeedbackRemoteSensorID = canCoderID;
+            turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
+            turnConfig.Feedback.SensorToMechanismRatio = 1.0;
+            turnConfig.Feedback.RotorToSensorRatio = Constants.Drivetrain.TURN_GEAR_RATIO;
 
             canCoderConfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id-1] + corner.offsetRots;
             canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
             canCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1;
 
-            this.driveMotor = new TalonFX(30 + id, canbus);
+            this.driveMotor = new TalonFX(driveID, canbus);
             this.driveMotor.getConfigurator().apply(driveConfig);
 
-            this.turnMotor = new TalonFX(20 + id, canbus);
+            this.cancoder = new CANcoder(canCoderID, canbus);
+            this.cancoder.getConfigurator().apply(canCoderConfig);
 
+            this.turnMotor = new TalonFX(turnID, canbus);
             this.turnMotor.getConfigurator().apply(turnConfig);
-
-            this.cancoder = new CANcoder(10 + id, canbus);
-
-            this.cancoder.getConfigurator().apply(canCoderConfig);
         }
     }
 }