Adding a preliminary mechanism for auto rezeroing the turning encoder from the absolute encoder when it's not moving

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -11,6 +11,7 @@ import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.util.Units;
 
 public class ModuleConstants {
@@ -51,6 +52,9 @@ public class ModuleConstants {
     public static final double kTurnI = 0;
     public static final double kTurnD = 0;
 
+    // TODO How sensitive is too sensitive?
+    public static final double kAutoResetPositionDeadband = Units.degreesToRadians(.25);
+    
     public static final int kTurnMotorCurrentLimit = 20;
 
     public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
@@ -98,5 +102,9 @@ 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/utilities/SwerveModule.java

@@ -43,7 +43,10 @@ public class SwerveModule {
 
     private String moduleName;
 
+    private SwerveModuleState lastTargetState;
+
     private boolean isAbsoluteEncoderDisabled;
+    private boolean turningEncoderAutoRezeroEnabled;
 
     /**
      * Builds the swerve module but with the Absolute Encoder disabled.
@@ -60,7 +63,8 @@ public class SwerveModule {
     }
 
     /**
-     * Builds the swerve module with the normal features
+     * Builds the swerve module with the normal features, disables automatic rezeroing of the turning encoder
+     * from the absolute encoder. 
      * 
      * @param moduleName The module name, Front Left, Front Right, etc.
      * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
@@ -69,6 +73,22 @@ public class SwerveModule {
      * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
      */
     public SwerveModule(String moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
+        this(moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false);
+    }
+
+    /**
+     * Builds the swerve module with the normal features, and gives the option to enable automatic turning encoder rezeroing
+     * when the turning motor is not moving
+     * 
+     * @param moduleName The module name, Front Left, Front Right, etc.
+     * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
+     * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
+     * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
+     * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
+     * @param turningEncoderAutoRezeroEnabled Should the turning encoder in the NEO automatically rezero from the absolute encoder
+     */
+    public SwerveModule(String moduleName, int drivingCANID, int turningCANID, 
+        int analogEncoderID, double analogEncoderOffset, boolean turningEncoderAutoRezeroEnabled) {
         isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0);
 
         drive = new TalonFX(drivingCANID);
@@ -102,6 +122,10 @@ public class SwerveModule {
         
         drive.setPosition(0);
 
+        this.lastTargetState = getState();
+
+        this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled;
+
         this.moduleName = "Drivetrain/Modules/" + moduleName;
     }
 
@@ -109,6 +133,12 @@ public class SwerveModule {
         Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
         Logger.recordOutput(moduleName + "/SwerveModuleState", getState());
         Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
+
+        if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) {
+            if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) {
+                resetEncoders();
+            }
+        }
     }
 
     public SwerveModuleState getState() {
@@ -131,6 +161,8 @@ public class SwerveModule {
         // Probably doesn't *hurt* that it's here, but it may not be needed
         desiredState.optimize(new Rotation2d(turningRelativeEncoder.getPosition()));
 
+        lastTargetState = desiredState;
+
         drive.setControl(
             driveVelocityRequest.withVelocity(
                 desiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters