Pushing up changes that are related to us trying to fix rotation

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);
     }
 }

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);
 

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() {