Add Phoenix Pro features

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,8 +31,8 @@ public void updateInputs(ModuleIOInputs inputs) {
         inputs.driveAppliedVolts = driveAppliedVolts;
         inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps());
 
-        inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition);
-        inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad());
+        inputs.turnAbsolutePosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad());
+        inputs.turnPosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad());
         inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec();
         inputs.turnAppliedVolts = turnAppliedVolts;
         inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps());

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

@@ -1,6 +1,7 @@
 package org.team1540.robot2024.subsystems.drive;
 
 import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.controls.VoltageOut;
@@ -13,6 +14,7 @@
 import org.team1540.robot2024.util.swerve.SwerveFactory;
 
 import static org.team1540.robot2024.Constants.Drivetrain.*;
+import static org.team1540.robot2024.Constants.SwerveConfig.CAN_BUS;
 
 /**
  * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and
@@ -42,47 +44,76 @@ public class ModuleIOTalonFX implements ModuleIO {
     private final StatusSignal<Double> turnAppliedVolts;
     private final StatusSignal<Double> turnCurrent;
 
-    private final Rotation2d absoluteEncoderOffset;
+    private final boolean isCANFD;
 
     public ModuleIOTalonFX(SwerveFactory.SwerveModuleHW hw) {
-
         driveTalon = hw.driveMotor;
         turnTalon = hw.turnMotor;
         cancoder = hw.cancoder;
-        absoluteEncoderOffset = hw.cancoderOffset;
+
+        isCANFD = CANBus.isNetworkFD(CAN_BUS);
 
         setDriveBrakeMode(true);
         setTurnBrakeMode(true);
 
         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(50.0, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition, turnVelocity, turnAppliedVolts, turnCurrent);
+        BaseStatusSignal.setUpdateFrequencyForAll(
+                50.0,
+                driveVelocity,
+                driveAppliedVolts,
+                driveCurrent,
+                turnAbsolutePosition,
+                turnVelocity,
+                turnAppliedVolts,
+                turnCurrent);
         driveTalon.optimizeBusUtilization();
         turnTalon.optimizeBusUtilization();
     }
 
     @Override
     public void updateInputs(ModuleIOInputs inputs) {
-        BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition, turnPosition, turnVelocity, turnAppliedVolts, turnCurrent);
+        if (isCANFD) {
+            BaseStatusSignal.waitForAll(0.01, drivePosition, turnPosition);
+            BaseStatusSignal.refreshAll(
+                    driveVelocity,
+                    driveAppliedVolts,
+                    driveCurrent,
+                    turnAbsolutePosition,
+                    turnVelocity,
+                    turnAppliedVolts,
+                    turnCurrent);
+        } else {
+            BaseStatusSignal.refreshAll(
+                    drivePosition,
+                    driveVelocity,
+                    driveAppliedVolts,
+                    driveCurrent,
+                    turnAbsolutePosition,
+                    turnPosition,
+                    turnVelocity,
+                    turnAppliedVolts,
+                    turnCurrent);
+        }
 
         inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO;
         inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO;
         inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
         inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
 
-        inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()).plus(absoluteEncoderOffset);
-        inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO);
-        inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO;
+        inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble());
+        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,10 +5,13 @@
 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 edu.wpi.first.math.geometry.Rotation2d;
-import static org.team1540.robot2024.Constants.SwerveConfig.CAN_BUS;;
+
+import static org.team1540.robot2024.Constants.SwerveConfig.CAN_BUS;
+import static org.team1540.robot2024.Constants.Drivetrain.TURN_GEAR_RATIO;
+
 
 public class SwerveFactory {
     private static final double[] moduleOffsetsRots = new double[]{
@@ -28,21 +31,20 @@ public static SwerveModuleHW getModuleMotors(int id, SwerveCorner corner) {
 
     public enum SwerveCorner {
         FRONT_LEFT(0),
-        FRONT_RIGHT(90),
-        BACK_LEFT(270),
-        BACK_RIGHT(180);
+        FRONT_RIGHT(0.25),
+        BACK_LEFT(0.75),
+        BACK_RIGHT(0.5);
 
-        private final double offset;
-        SwerveCorner(double offset) {
-            this.offset = offset;
+        private final double offsetRots;
+        SwerveCorner(double offsetRots) {
+            this.offsetRots = offsetRots;
         }
     }
 
     public static class SwerveModuleHW {
         public final TalonFX driveMotor;
         public final TalonFX turnMotor;
         public final CANcoder cancoder;
-        public final Rotation2d cancoderOffset;
 
         private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
             if (id < 1 || id > 8) {
@@ -51,6 +53,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();
@@ -64,26 +71,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 = TURN_GEAR_RATIO;
 
-            canCoderConfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id-1];
+            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.turnMotor.getConfigurator().apply(turnConfig);
-
-            this.cancoder = new CANcoder(10 + id, canbus);
-
-
+            this.cancoder = new CANcoder(canCoderID, canbus);
             this.cancoder.getConfigurator().apply(canCoderConfig);
 
-            this.cancoderOffset = Rotation2d.fromDegrees(corner.offset);
-
-
+            this.turnMotor = new TalonFX(turnID, canbus);
+            this.turnMotor.getConfigurator().apply(turnConfig);
         }
     }
 }