From 4666a74877e16b9ed907c69e218514683918853c Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Sat, 31 Jan 2026 13:29:11 -0500
Subject: [PATCH] Pushing up changes that are related to us trying to fix
 rotation

---
 .../frc/robot/constants/ModuleConstants.java  |  4 ----
 .../java/frc/robot/subsystems/Drivetrain.java | 16 ++++++++--------
 .../frc/robot/utilities/SwerveModule.java     | 19 +++++++++++++++----
 3 files changed, 23 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java
index f422f20..1972ca4 100644
--- a/src/main/java/frc/robot/constants/ModuleConstants.java
+++ b/src/main/java/frc/robot/constants/ModuleConstants.java
@@ -101,9 +101,5 @@ public class ModuleConstants {
             .outputRange(-1, 1)
             .positionWrappingEnabled(true)
             .positionWrappingInputRange(0, 2 * Math.PI);
-        turningConfig.signals
-            .primaryEncoderVelocityAlwaysOn(false)
-            .primaryEncoderPositionAlwaysOn(true)
-            .primaryEncoderPositionPeriodMs(20);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 93cfc5c..7990836 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -48,34 +48,34 @@ public class Drivetrain extends SubsystemBase {
         frontLeft = new SwerveModule(
             "FrontLeft", 
             DrivetrainConstants.kFrontLeftDrivingCANID, 
-            DrivetrainConstants.kFrontLeftTurningCANID, 
+            DrivetrainConstants.kFrontLeftTurningCANID);/*, 
             DrivetrainConstants.kFrontLeftAnalogInPort, 
             DrivetrainConstants.kFrontLeftMagEncoderOffset
-        ); 
+        ); */
 
         frontRight = new SwerveModule(
             "FrontRight", 
             DrivetrainConstants.kFrontRightDrivingCANID, 
-            DrivetrainConstants.kFrontRightTurningCANID, 
+            DrivetrainConstants.kFrontRightTurningCANID);/*, 
             DrivetrainConstants.kFrontRightAnalogInPort, 
             DrivetrainConstants.kFrontRightMagEncoderOffset
-        );
+        );*/
 
         rearLeft = new SwerveModule(
             "RearLeft", 
             DrivetrainConstants.kRearLeftDrivingCANID, 
-            DrivetrainConstants.kRearLeftTurningCANID, 
+            DrivetrainConstants.kRearLeftTurningCANID);/*, 
             DrivetrainConstants.kRearLeftAnalogInPort, 
             DrivetrainConstants.kRearLeftMagEncoderOffset
-        );
+        ); */
 
         rearRight = new SwerveModule(
             "RearRight", 
             DrivetrainConstants.kRearRightDrivingCANID, 
-            DrivetrainConstants.kRearRightTurningCANID, 
+            DrivetrainConstants.kRearRightTurningCANID);/*, 
             DrivetrainConstants.kRearRightAnalogInPort, 
             DrivetrainConstants.kRearRightMagEncoderOffset
-        );
+        ); */
 
         gyro = new AHRS(NavXComType.kMXP_SPI);
 
diff --git a/src/main/java/frc/robot/utilities/SwerveModule.java b/src/main/java/frc/robot/utilities/SwerveModule.java
index e5a67c4..b684967 100644
--- a/src/main/java/frc/robot/utilities/SwerveModule.java
+++ b/src/main/java/frc/robot/utilities/SwerveModule.java
@@ -12,6 +12,7 @@ import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -44,6 +45,7 @@ public class SwerveModule {
     private String moduleName;
 
     private SwerveModuleState lastTargetState;
+    private SwerveModuleState lastTargetStateOptimized;
 
     private boolean isAbsoluteEncoderDisabled;
     private boolean turningEncoderAutoRezeroEnabled;
@@ -125,6 +127,7 @@ public class SwerveModule {
         drive.setPosition(0);
 
         this.lastTargetState = getState();
+        this.lastTargetStateOptimized = getState();
 
         this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled;
 
@@ -136,16 +139,18 @@ public class SwerveModule {
             Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
         }
         
-        Logger.recordOutput(moduleName + "ModuleTargetState", lastTargetState);
+        Logger.recordOutput(moduleName + "/ModuleTargetState", lastTargetState);
+        Logger.recordOutput(moduleName + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
         Logger.recordOutput(moduleName + "/SwerveModuleState", getState());
         Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
         Logger.recordOutput(moduleName + "/RelativeEncoderPosition", getTurningEncoderPosition());
 
+        /*
         if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) {
             if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) {
                 resetEncoders();
             }
-        }
+        }*/
     }
 
     public SwerveModuleState getState() {
@@ -168,12 +173,13 @@ public class SwerveModule {
     }
 
     public void setDesiredState(SwerveModuleState desiredState) {
+        lastTargetState = new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
         // TODO is this really necessary, the offset is managed by the Absolute Encoder
         // and its "source of truth" behavior in relation to the relative encoder
         // Probably doesn't *hurt* that it's here, but it may not be needed
         desiredState.optimize(new Rotation2d(getTurningEncoderPosition()));
 
-        lastTargetState = desiredState;
+        lastTargetStateOptimized = desiredState;
 
         drive.setControl(
             driveVelocityRequest.withVelocity(
@@ -190,7 +196,12 @@ public class SwerveModule {
     }
 
     public double getTurningEncoderPosition() {
-        return turningRelativeEncoder.getPosition() * (ModuleConstants.kIsEncoderInverted ? -1 : 1);
+        return MathUtil.inputModulus(
+            turningRelativeEncoder.getPosition(), 
+            0, 
+            2 * Math.PI
+        );
+        //return turningRelativeEncoder.getPosition() * (ModuleConstants.kIsEncoderInverted ? -1 : 1);
     }
 
     public void resetEncoders() {