Reduce odometry teleportation

src/main/java/frc/robot/RobotContainer.java

@@ -262,7 +262,7 @@ private boolean errorsPresent() {
   }
 
   public void updateSwerveOdometry() {
-    swerve.updateOdometryQueue();
+    swerve.updateOdometry();
   }
 
   /**

src/main/java/frc/robot/subsystems/Swerve.java

@@ -21,7 +21,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.numbers.N3;
@@ -54,7 +53,6 @@
 import frc.robot.util.LimelightHelpers;
 import frc.robot.util.LimelightHelpers.LimelightTarget_Fiducial;
 import java.util.ArrayList;
-import java.util.Collections;
 import java.util.List;
 import java.util.Optional;
 import java.util.concurrent.locks.Lock;
@@ -104,11 +102,6 @@ public class Swerve extends VirtualSubsystem {
   public static final Lock odometryLock = new ReentrantLock();
   private SwerveDrivePoseEstimator poseEstimator;
 
-  private List<SwerveDriveWheelPositions> wheelPositionsQueue =
-      Collections.synchronizedList(new ArrayList<>(4));
-  private List<Rotation2d> gyroAnglesQueue = Collections.synchronizedList(new ArrayList<>(4));
-  private List<Double> timestampsQueue = Collections.synchronizedList(new ArrayList<>(4));
-
   private double limelightLastDetectedTime = 0.0;
 
   private final SysIdRoutine sysIdRoutine =
@@ -335,7 +328,10 @@ public ChassisSpeeds getChassisSpeeds() {
   }
 
   public Pose2d getPose() {
-    return poseEstimator.getEstimatedPosition();
+    odometryLock.lock();
+    Pose2d pose = poseEstimator.getEstimatedPosition();
+    odometryLock.unlock();
+    return pose;
   }
 
   public void resetPose(Pose2d pose) {
@@ -373,30 +369,16 @@ public SwerveModuleState[] getDesiredStates() {
     };
   }
 
-  public void updateOdometryQueue() {
-    odometryLock.lock();
-    try {
-      wheelPositionsQueue.add(new SwerveDriveWheelPositions(getModulePositions()));
-      gyroAnglesQueue.add(getHeading());
-      timestampsQueue.add(Timer.getFPGATimestamp());
-    } finally {
-      odometryLock.unlock();
-    }
-  }
-
   public void updateOdometry() {
-    int newEntries =
-        Math.min(
-            wheelPositionsQueue.size(), Math.min(gyroAnglesQueue.size(), timestampsQueue.size()));
-
-    for (int i = 0; i < newEntries; i++) {
-      poseEstimator.updateWithTime(
-          timestampsQueue.get(i), gyroAnglesQueue.get(i), wheelPositionsQueue.get(i));
+    if (!frontLeftModule.motorsValid()
+        || !frontRightModule.motorsValid()
+        || !backLeftModule.motorsValid()
+        || !backRightModule.motorsValid()) {
+      return;
     }
-
-    wheelPositionsQueue.clear();
-    gyroAnglesQueue.clear();
-    timestampsQueue.clear();
+    odometryLock.lock();
+    poseEstimator.update(getHeading(), getModulePositions());
+    odometryLock.unlock();
   }
 
   private Vector<N3> getLLStandardDeviations(
@@ -499,21 +481,16 @@ public void updateVisionPoseEstimates() {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    odometryLock.lock();
     frontLeftModule.periodic();
     frontRightModule.periodic();
     backLeftModule.periodic();
     backRightModule.periodic();
 
-    odometryLock.lock();
-    try {
-      updateOdometry();
-    } finally {
-      odometryLock.unlock();
-    }
-
     updateVisionPoseEstimates();
 
     field.setRobotPose(poseEstimator.getEstimatedPosition());
+    odometryLock.unlock();
   }
 
   @Override

src/main/java/frc/robot/subsystems/SwerveModule.java

@@ -19,6 +19,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
@@ -55,6 +56,9 @@ public class SwerveModule {
   private SwerveModuleState desiredState = new SwerveModuleState();
   private SwerveModuleState previousState = new SwerveModuleState();
 
+  private double previousDrivePosition = 0.0;
+  private double previousTurnPosition = 0.0;
+
   private double characterizationVolts = 0.0;
   private boolean characterizing = false;
 
@@ -260,6 +264,40 @@ public Rotation2d getAbsoluteAngle() {
     return Rotation2d.fromRotations(absoluteAngleSignal.getValue());
   }
 
+  public boolean driveMotorValid() {
+    double position = driveEncoder.getPosition();
+    double positionDelta = position - previousDrivePosition;
+    double velocity = getVelocity();
+
+    if (Double.isNaN(position) || Double.isInfinite(position)) {
+      return false;
+    }
+
+    if (Math.abs(positionDelta) < Math.abs(velocity * 1.15)) {
+      if (Math.abs(velocity) < 1.0e-4) {
+        return true;
+      }
+      return true;
+    }
+
+    return Math.signum(positionDelta) == Math.signum(velocity) && Math.abs(velocity) > 1.0e-4;
+  }
+
+  public boolean turnMotorValid() {
+    double position = turnEncoder.getPosition();
+    double positionDelta = position - previousTurnPosition;
+
+    if (Double.isNaN(position) || Double.isInfinite(position)) {
+      return false;
+    }
+
+    return Math.abs(positionDelta) < Units.degreesToRadians(60.0);
+  }
+
+  public boolean motorsValid() {
+    return driveMotorValid() && turnMotorValid();
+  }
+
   private void setSpeed(double speedMetersPerSecond) {
     if (isOpenLoop) {
       driveMotor.set(speedMetersPerSecond / SwerveConstants.maxModuleSpeed);
@@ -293,6 +331,9 @@ public void periodic() {
       setAngle(Rotation2d.fromDegrees(0.0));
     }
 
+    previousDrivePosition = driveEncoder.getPosition();
+    previousTurnPosition = turnEncoder.getPosition();
+
     updateTelemetry();
   }