Attempt to fix Odometry inaccuracies

build.gradle

@@ -15,7 +15,7 @@ java {
 }
 
 // set a valid semver version
-version = '4.1.2'
+version = '4.1.3-test1'
 
 repositories {
     mavenCentral()

src/main/java/ca/team1310/swerve/SwerveTelemetry.java

@@ -78,6 +78,12 @@ public final class SwerveTelemetry {
   /** The drive motor output power of the swerve modules */
   public double[] driveMotorOutputPower;
 
+  /**
+   * Per-module angle error in degrees: difference between relative encoder angle and absolute
+   * encoder angle. Useful for monitoring encoder drift.
+   */
+  public double[] moduleAngleErrorDegrees;
+
   // Pose
   /** The x location of the robot with respect to the field in metres */
   public double poseMetresX = Double.MIN_VALUE;
@@ -145,6 +151,7 @@ public SwerveTelemetry(int moduleCount) {
     moduleAngleMotorPositionDegrees = new double[moduleCount];
     moduleDriveMotorPositionMetres = new double[moduleCount];
     driveMotorOutputPower = new double[moduleCount];
+    moduleAngleErrorDegrees = new double[moduleCount];
   }
 
   /** Post all telemetry data to SmartDashboard */
@@ -221,5 +228,7 @@ private void postVerbose() {
       double pwr = driveMotorOutputPower[i];
       SmartDashboard.putString(PREFIX + "Swerve/drive_power_" + name, String.format("%.3f", pwr));
     }
+
+    SmartDashboard.putNumberArray(PREFIX + "Swerve/angleErrors", moduleAngleErrorDegrees);
   }
 }

src/main/java/ca/team1310/swerve/core/AbsoluteAngleEncoder.java

@@ -12,6 +12,20 @@ public interface AbsoluteAngleEncoder {
    */
   double getPosition();
 
+  /**
+   * Get the cached absolute location of the encoder without performing expensive refresh or health
+   * check operations. The cached value comes from the StatusSignal's auto-refresh (typically
+   * 100Hz), so it is at most 10ms old.
+   *
+   * <p>This is suitable for high-frequency reads such as odometry updates where latency of a full
+   * refresh is unacceptable.
+   *
+   * @return The absolute location of the encoder in degrees, from 0 to 360. Returns -1 on error.
+   */
+  default double getPositionCached() {
+    return getPosition();
+  }
+
   /**
    * Are there any active faults on this motor
    *

src/main/java/ca/team1310/swerve/core/AngleMotor.java

@@ -34,6 +34,20 @@ public interface AngleMotor {
    */
   void setEncoderPosition(double actualAngleDegrees);
 
+  /**
+   * Gradually correct the internal encoder position toward the absolute angle. Unlike {@link
+   * #setEncoderPosition(double)}, this applies a fractional correction to avoid discontinuities in
+   * the PID feedback signal during active steering.
+   *
+   * <p>The default implementation delegates to {@link #setEncoderPosition(double)}.
+   *
+   * @param absoluteAngleDegrees the true angle from the absolute encoder (0 to 360)
+   * @param correctionFactor fraction of the error to apply per call (e.g. 0.2 = 20%)
+   */
+  default void correctEncoderPosition(double absoluteAngleDegrees, double correctionFactor) {
+    setEncoderPosition(absoluteAngleDegrees);
+  }
+
   /**
    * Are there any active faults on this motor
    *

src/main/java/ca/team1310/swerve/core/CoreSwerveDrive.java

@@ -5,6 +5,7 @@
 import ca.team1310.swerve.RunnymedeSwerveDrive;
 import ca.team1310.swerve.SwerveTelemetry;
 import ca.team1310.swerve.core.config.CoreSwerveConfig;
+import ca.team1310.swerve.utils.SwerveUtils;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotBase;
 
@@ -50,6 +51,9 @@ public class CoreSwerveDrive implements RunnymedeSwerveDrive {
 
   private final Notifier moduleManagementThread = new Notifier(this::updateModules);
 
+  private static final int SYNC_ENCODERS_PERIOD_MS = 500;
+  private final Notifier encoderSyncThread = new Notifier(this::syncAllEncoders);
+
   public static final int TELEMETRY_UPDATE_PERIOD_MS = 50; // milliseconds
   private final Notifier telemetryThread = new Notifier(this::updateTelemetry);
 
@@ -61,6 +65,7 @@ public class CoreSwerveDrive implements RunnymedeSwerveDrive {
   protected CoreSwerveDrive(CoreSwerveConfig cfg) {
     System.out.println("Initializing RunnymedeSwerve.");
     System.out.println("Swerve module update period: " + MANAGE_MODULES_PERIOD_MS + " ms");
+    System.out.println("Swerve encoder sync period: " + SYNC_ENCODERS_PERIOD_MS + " ms");
     System.out.println("Swerve telemetry update period: " + TELEMETRY_UPDATE_PERIOD_MS + " ms");
 
     // order matters in case we want to use AdvantageScope
@@ -121,6 +126,9 @@ protected CoreSwerveDrive(CoreSwerveConfig cfg) {
     moduleManagementThread.setName("RunnymedeSwerve manageModuleStates");
     moduleManagementThread.startPeriodic(MANAGE_MODULES_PERIOD_MS / 1000.0);
 
+    encoderSyncThread.setName("RunnymedeSwerve syncEncoders");
+    encoderSyncThread.startPeriodic(SYNC_ENCODERS_PERIOD_MS / 1000.0);
+
     telemetryThread.setName("RunnymedeSwerve updateTelemetry");
     // in simulation mode, provide telemetry faster but while driving use slower rate
     telemetryThread.startPeriodic(isSimulation ? .02 : TELEMETRY_UPDATE_PERIOD_MS / 1000.0);
@@ -161,6 +169,13 @@ protected synchronized void updateModules() {
     }
   }
 
+  /** Sync relative encoders toward absolute encoders on a separate, slower thread. */
+  private synchronized void syncAllEncoders() {
+    for (SwerveModule module : modules) {
+      module.syncEncoders();
+    }
+  }
+
   /** Update the gyro in case the robot is running in simulation mode. */
   protected void updateGyroForSimulation() {}
 
@@ -293,6 +308,14 @@ protected synchronized void updateTelemetry(SwerveTelemetry telemetry) {
         telemetry.driveMotorOutputPower[i] = state.getDriveOutputPower();
         // angle encoder
         telemetry.moduleAbsoluteEncoderPositionDegrees[i] = state.getAbsoluteEncoderAngle();
+
+        // Compute per-module angle error (relative vs absolute encoder)
+        double absAngle = state.getAbsoluteEncoderAngle();
+        if (absAngle >= 0) {
+          double absNormalized = SwerveUtils.normalizeDegrees(absAngle);
+          double error = SwerveUtils.normalizeDegrees(state.getAngle() - absNormalized);
+          telemetry.moduleAngleErrorDegrees[i] = error;
+        }
       }
     }
     // post it!

src/main/java/ca/team1310/swerve/core/ModuleState.java

@@ -1,5 +1,7 @@
 package ca.team1310.swerve.core;
 
+import static ca.team1310.swerve.utils.SwerveUtils.normalizeDegrees;
+
 import ca.team1310.swerve.utils.Coordinates;
 
 /**
@@ -89,6 +91,20 @@ public double getAngle() {
     return anglePosition;
   }
 
+  /**
+   * Get the best available angle for odometry. Returns the absolute encoder angle (normalized to
+   * -180..180) if available, falling back to the relative encoder angle. The absolute encoder is
+   * preferred because the relative encoder can drift between sync cycles.
+   *
+   * @return the angle in degrees from -180 to 180 (ccw positive)
+   */
+  public double getOdometryAngle() {
+    if (absoluteEncoderAngle >= 0) {
+      return normalizeDegrees(absoluteEncoderAngle);
+    }
+    return anglePosition;
+  }
+
   /**
    * Get the speed of the module
    *

src/main/java/ca/team1310/swerve/core/SwerveModule.java

@@ -37,6 +37,12 @@ public interface SwerveModule {
    */
   void readState();
 
+  /**
+   * Synchronize the relative encoder toward the absolute encoder. Called every module update cycle
+   * after {@link #readState()}.
+   */
+  void syncEncoders();
+
   /**
    * Update the internal state of the swerve module's detailed telemetry information. It does NOT
    * include data loaded by readState(). Some of these operations can be slow and should therefore

src/main/java/ca/team1310/swerve/core/SwerveModuleImpl.java

@@ -9,12 +9,13 @@
 import ca.team1310.swerve.math.SwerveMath;
 import ca.team1310.swerve.utils.Coordinates;
 import edu.wpi.first.wpilibj.Alert;
-import edu.wpi.first.wpilibj.Notifier;
 
 class SwerveModuleImpl implements SwerveModule {
 
-  private static final double ANGLE_ENCODER_SYNC_PERIOD_MS = 500;
-  private final Notifier encoderSynchronizer = new Notifier(this::syncAngleEncoder);
+  private static final boolean USE_GRADUAL_ENCODER_CORRECTION = true;
+  private static final double ENCODER_CORRECTION_FACTOR = 0.1;
+  private static final int HARD_SYNC_CYCLE_INTERVAL =
+      500 / CoreSwerveDrive.MANAGE_MODULES_PERIOD_MS; // ~50 cycles = 500ms
 
   private final String name;
   private final Coordinates location;
@@ -23,28 +24,20 @@ class SwerveModuleImpl implements SwerveModule {
   private final AbsoluteAngleEncoder angleEncoder;
   private ModuleDirective desiredState = new ModuleDirective();
   private final ModuleState measuredState = new ModuleState();
+  private int hardSyncCycleCounter = 0;
 
   private final Alert driveMotorFaultPresent;
   private final Alert angleMotorFaultPresent;
   private final Alert angleEncoderFaultPresent;
 
   SwerveModuleImpl(ModuleConfig cfg, double maxAttainableModuleSpeedMps) {
     this.name = cfg.name();
-    System.out.println(
-        "Swerve ("
-            + this.name
-            + ") absolute angle encoder sync period: "
-            + ANGLE_ENCODER_SYNC_PERIOD_MS
-            + " ms");
     this.location = cfg.location();
     measuredState.setLocation(cfg.location());
     this.driveMotor = getDriveMotor(cfg, maxAttainableModuleSpeedMps);
     this.angleMotor = getAngleMotor(cfg);
     this.angleEncoder = getAbsoluteAngleEncoder(cfg);
 
-    this.encoderSynchronizer.setName("RunnymedeSwerve Angle Encoder Sync " + name);
-    this.encoderSynchronizer.startPeriodic(ANGLE_ENCODER_SYNC_PERIOD_MS / 1000);
-
     driveMotorFaultPresent =
         new Alert("Swerve Drive Motor [" + name + "] Fault Present", Alert.AlertType.kError);
     angleMotorFaultPresent =
@@ -89,10 +82,6 @@ private AbsoluteAngleEncoder getAbsoluteAngleEncoder(ModuleConfig cfg) {
         cfg.absoluteAngleEncoderConfig());
   }
 
-  private synchronized void syncAngleEncoder() {
-    angleMotor.setEncoderPosition(angleEncoder.getPosition());
-  }
-
   public String getName() {
     return name;
   }
@@ -103,6 +92,20 @@ public synchronized void readState() {
 
     measuredState.setAngle(angleMotor.getPosition());
     measuredState.setPosition(driveMotor.getDistance());
+    measuredState.setAbsoluteEncoderAngle(angleEncoder.getPositionCached());
+  }
+
+  public synchronized void syncEncoders() {
+    double absoluteEncoderAngle = measuredState.getAbsoluteEncoderAngle();
+    if (absoluteEncoderAngle < 0) {
+      return; // invalid CANCoder reading (-1 means error)
+    }
+    if (USE_GRADUAL_ENCODER_CORRECTION) {
+      angleMotor.correctEncoderPosition(absoluteEncoderAngle, ENCODER_CORRECTION_FACTOR);
+    } else if (++hardSyncCycleCounter >= HARD_SYNC_CYCLE_INTERVAL) {
+      hardSyncCycleCounter = 0;
+      angleMotor.setEncoderPosition(absoluteEncoderAngle);
+    }
   }
 
   public synchronized void readVerboseState() {
@@ -118,7 +121,7 @@ public synchronized ModuleState getState() {
   public synchronized void setDesiredState(ModuleDirective desiredState) {
     this.desiredState = desiredState;
 
-    double currentHeadingDeg = angleMotor.getPosition();
+    double currentHeadingDeg = measuredState.getOdometryAngle();
     SwerveMath.optimizeWheelAngles(desiredState, currentHeadingDeg);
     SwerveMath.cosineCompensator(desiredState, currentHeadingDeg);
 

src/main/java/ca/team1310/swerve/core/SwerveModuleSimulation.java

@@ -38,6 +38,9 @@ public Coordinates getLocation() {
   @Override
   public void readState() {}
 
+  @Override
+  public void syncEncoders() {}
+
   @Override
   public void readVerboseState() {}
 

src/main/java/ca/team1310/swerve/core/hardware/cancoder/CanCoder.java

@@ -100,6 +100,14 @@ public double getPosition() {
     return measuredPosition;
   }
 
+  @Override
+  public double getPositionCached() {
+    if (angle.getStatus() != StatusCode.OK) {
+      return -1;
+    }
+    return ((angle.getValue().in(Degrees) - absoluteEncoderOffset) + 360) % 360;
+  }
+
   private double calculatePosition() {
     MagnetHealthValue strength = magnetHealth.refresh().getValue();
 

src/main/java/ca/team1310/swerve/core/hardware/rev/neospark/NSAngleMotor.java

@@ -119,6 +119,17 @@ public void setReferenceAngle(double degrees) {
     doWithRetry(() -> controller.setReference(degrees, SparkBase.ControlType.kPosition));
   }
 
+  @Override
+  public void correctEncoderPosition(double absoluteAngleDegrees, double correctionFactor) {
+    double rawPosition = encoder.getPosition();
+    double normalizedPosition = normalizeDegrees(rawPosition);
+    double error = normalizeDegrees(absoluteAngleDegrees - normalizedPosition);
+    if (Math.abs(error) < 0.1) {
+      return; // dead band: avoid CAN traffic for negligible corrections
+    }
+    encoder.setPosition(rawPosition + correctionFactor * error); // fire-and-forget
+  }
+
   @Override
   public void setEncoderPosition(double actualAngleDegrees) {
     double omega = Math.abs(encoder.getVelocity());

src/main/java/ca/team1310/swerve/odometry/FieldAwareSwerveDrive.java

@@ -106,7 +106,7 @@ private synchronized SwerveModulePosition[] getSwerveModulePositions() {
     var states = getModuleStates();
     for (int i = 0; i < states.length; i++) {
       modulePosition[i].distanceMeters = states[i].getPosition();
-      modulePosition[i].angle = Rotation2d.fromDegrees(states[i].getAngle());
+      modulePosition[i].angle = Rotation2d.fromDegrees(states[i].getOdometryAngle());
     }
     return modulePosition;
   }