From 447101cb0d671c10feae9d34e8dea6c062561cb0 Mon Sep 17 00:00:00 2001
From: Alex Resnick <adr8292@gmail.com>
Date: Sun, 15 Dec 2024 10:05:53 -0600
Subject: [PATCH] Fused CANCoder works

---
 .../java/frc/lib/util/swerve/SwerveModule.java   |  4 +---
 .../frc/lib/util/swerve/SwerveModuleReal.java    | 16 ++++++++++++++--
 .../java/frc/robot/subsystems/swerve/Swerve.java |  9 ---------
 .../frc/robot/subsystems/swerve/SwerveReal.java  |  2 +-
 4 files changed, 16 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java
index e3aa5e3..d6ba78f 100644
--- a/src/main/java/frc/lib/util/swerve/SwerveModule.java
+++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java
@@ -34,7 +34,6 @@ public SwerveModule(int moduleNumber, Rotation2d angleOffset, SwerveModuleIO io)
 
         // lastAngle = getState().angle.getDegrees();
         io.updateInputs(inputs);
-        resetToAbsolute();
         Logger.processInputs("SwerveModule" + moduleNumber, inputs);
     }
 
@@ -119,8 +118,7 @@ public SwerveModuleState getStateAbs() {
         return new SwerveModuleState(
             Conversions.rotationPerSecondToMetersPerSecond(inputs.driveMotorSelectedSensorVelocity,
                 Constants.Swerve.wheelCircumference),
-            Rotation2d.fromRotations(
-                inputs.absolutePositionAngleEncoder - this.angleOffset.getRotations()));
+            Rotation2d.fromRotations(inputs.absolutePositionAngleEncoder));
     }
 
     /**
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
index 8332be8..f266378 100644
--- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
+++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
@@ -11,6 +11,8 @@
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import frc.lib.math.Conversions;
@@ -40,9 +42,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);
         mDriveMotor = new TalonFX(driveMotorID);
         mAngleMotor = new TalonFX(angleMotorID);
@@ -63,8 +69,12 @@ private void configAngleMotor() {
         swerveAngleFXConfig.MotorOutput.Inverted = Constants.Swerve.angleMotorInvert;
         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 */
@@ -127,6 +137,8 @@ private void configDriveMotor() {
     private void configAngleEncoder() {
         /* Angle Encoder Config */
         swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert;
+        swerveCANcoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
+        swerveCANcoderConfig.MagnetSensor.MagnetOffset = -angleOffset.getRotations();
 
         angleEncoder.getConfigurator().apply(swerveCANcoderConfig);
     }
diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
index dbb2620..b243698 100644
--- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java
+++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
@@ -236,15 +236,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();
-        }
-    }
-
     @Override
     public void periodic() {
         // Robot.profiler.push("swerve_periodic");
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
index 80d4315..63c937b 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
@@ -30,7 +30,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