Use fused CANCoder mode

README.md

@@ -1,12 +1,13 @@
 # FRC2024
+
 [![CI](https://github.com/Frc5572/FRC2024/actions/workflows/main.yml/badge.svg)](https://github.com/Frc5572/FRC2024/actions/workflows/main.yml) [![Java Docs](https://img.shields.io/badge/docs-2024-blue)](https://frc5572.github.io/FRC2024/)
 
 Code for our 2024 robot
 
-
 ## CAN IDs
 
 ### Swerve
+
 Front Left Drive Motor - 10  
 Front Left Angle Motor - 8  
 Front Left canCoder - 10  
@@ -21,6 +22,7 @@ Back Right Angle Motor - 51
 Back Right canCoder - 4  
 
 ### Other
+
 Shooter Top - 13  
 Shooter Bottom - 15  
 Elevator - 57  
@@ -30,14 +32,12 @@ Indexer - 32
 Climber Left - 60  
 Climber Right - 48  
 
-
 ## PDH Ports
 
 ### Swerve
 
-
-
 ### Other
+
 0 - Wrist  
 1 - Left Climber  
 2 - Elevator  

src/main/java/frc/lib/util/swerve/SwerveModule.java

@@ -35,7 +35,6 @@ public SwerveModule(int moduleNumber, Rotation2d angleOffset, SwerveModuleIO io)
 
         // lastAngle = getState().angle.getDegrees();
         io.updateInputs(inputs);
-        resetToAbsolute();
         Logger.processInputs("SwerveModule" + moduleNumber, inputs);
     }
 
@@ -90,15 +89,6 @@ public Rotation2d getCANcoder() {
         return Rotation2d.fromRotations(inputs.absolutePositionAngleEncoder);
     }
 
-    /**
-     * Reset the Swerve Module angle to face forward
-     */
-    public void resetToAbsolute() {
-        double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations();
-        io.setPositionAngleMotor(absolutePosition);
-        inputs.angleMotorSelectedPosition = absolutePosition;
-    }
-
     /**
      * Get the current Swerve Module State
      *

src/main/java/frc/lib/util/swerve/SwerveModuleReal.java

@@ -9,6 +9,9 @@
 import com.ctre.phoenix6.controls.VelocityVoltage;
 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 edu.wpi.first.math.geometry.Rotation2d;
 import frc.lib.math.Conversions;
 import frc.robot.Constants;
 
@@ -35,9 +38,13 @@ public class SwerveModuleReal implements SwerveModuleIO {
 
     /* angle motor control requests */
     private final PositionVoltage anglePosition = new PositionVoltage(0);
+    private final Rotation2d angleOffset;
 
     /** Instantiating motors and Encoders */
-    public SwerveModuleReal(int driveMotorID, int angleMotorID, int cancoderID) {
+    public SwerveModuleReal(int driveMotorID, int angleMotorID, int cancoderID,
+        Rotation2d cancoderOffset) {
+
+        this.angleOffset = cancoderOffset;
 
         angleEncoder = new CANcoder(cancoderID, "canivore");
         mDriveMotor = new TalonFX(driveMotorID, "canivore");
@@ -60,7 +67,10 @@ private void configAngleMotor() {
         swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode;
 
         /* Gear Ratio and Wrapping Config */
-        swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio;
+        swerveAngleFXConfig.Feedback.FeedbackRemoteSensorID = angleEncoder.getDeviceID();
+        swerveAngleFXConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
+        swerveAngleFXConfig.Feedback.SensorToMechanismRatio = 1.0;
+        swerveAngleFXConfig.Feedback.RotorToSensorRatio = Constants.Swerve.angleGearRatio;
         swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true;
 
         /* Current Limiting */
@@ -123,6 +133,9 @@ private void configDriveMotor() {
     private void configAngleEncoder() {
         /* Angle Encoder Config */
         swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert;
+        swerveCANcoderConfig.MagnetSensor.AbsoluteSensorRange =
+            AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
+        swerveCANcoderConfig.MagnetSensor.MagnetOffset = -angleOffset.getRotations();
 
         angleEncoder.getConfigurator().apply(swerveCANcoderConfig);
     }

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

@@ -225,15 +225,6 @@ public void resetFieldRelativeOffset() {
         fieldOffset = getGyroYaw().getDegrees() + 180;
     }
 
-    /**
-     * Reset all modules to their front facing position
-     */
-    public void resetModulesToAbsolute() {
-        for (SwerveModule mod : swerveMods) {
-            mod.resetToAbsolute();
-        }
-    }
-
     public void resetPvInitialization() {
         hasInitialized = false;
     }

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

@@ -25,7 +25,7 @@ public void updateInputs(SwerveInputs inputs) {
     public SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, int angleMotorID,
         int cancoderID, Rotation2d angleOffset) {
         return new SwerveModule(moduleNumber, angleOffset,
-            new SwerveModuleReal(driveMotorID, angleMotorID, cancoderID));
+            new SwerveModuleReal(driveMotorID, angleMotorID, cancoderID, angleOffset));
     }
 
     @Override