Fixed Odomety bug

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

@@ -26,6 +26,7 @@
 import frc.robot.commands.FeedForwardCharacterization;
 import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.kitbotshooter.*;
+import frc.robot.util.AllianceFlipUtil;
 import frc.robot.util.trajectory.ChoreoTrajectoryReader;
 import frc.robot.util.trajectory.Trajectory;
 import java.io.File;
@@ -125,7 +126,9 @@ public RobotContainer() {
                       Commands.runOnce(
                           () ->
                               robotState.resetPose(
-                                  traj.startPose(), drive.getWheelPositions(), drive.getGyroYaw()),
+                                  AllianceFlipUtil.apply(traj.startPose()),
+                                  drive.getWheelPositions(),
+                                  drive.getGyroYaw()),
                           drive),
                       new DriveTrajectory(drive, traj)));
         };
@@ -166,7 +169,7 @@ private void configureButtonBindings() {
                             .resetPose(
                                 new Pose2d(
                                     robotState.getEstimatedPose().getTranslation(),
-                                    new Rotation2d()),
+                                    AllianceFlipUtil.apply(new Rotation2d())),
                                 drive.getWheelPositions(),
                                 drive.getGyroYaw()),
                     drive)

src/main/java/frc/robot/RobotState.java

@@ -17,10 +17,7 @@
 public class RobotState {
   /** Odometry data from one main robot loop cycle */
   public record OdometryObservation(
-      SwerveDriveWheelPositions[] wheelPositions,
-      Rotation2d[] gyroAngles,
-      double[] timestamps,
-      boolean gyroConnected) {}
+      SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) {}
 
   public record VisionObservation(Pose2d visionPose, double timestamp, Matrix<N3, N1> stdDevs) {}
 
@@ -52,56 +49,29 @@ public static RobotState getInstance() {
   private Rotation2d lastGyroAngle = new Rotation2d();
 
   private RobotState() {
-    odometryStdDevs.setColumn(0, DriveConstants.odometryStateStDevs.extractColumnVector(0));
+    odometryStdDevs.setColumn(0, DriveConstants.odometryStateStdDevs.extractColumnVector(0));
     kinematics = DriveConstants.kinematics;
   }
 
   public void addOdometryData(OdometryObservation observation) {
     Pose2d lastOdometryPose = odometryPose;
-    int minDelta = observation.wheelPositions().length;
-    if (observation.gyroConnected) minDelta = Math.min(minDelta, observation.gyroAngles().length);
-    for (int deltaIndex = 0; deltaIndex < minDelta; deltaIndex++) {
-      //      double[] turnPositions = new double[4];
-      //      double[] lastTurnPositions = new double[4];
-      //      double[] turnPositionDeltas = new double[4];
-      //      for (int i = 0; i < 4; i++) {
-      //        turnPositions[i] =
-      // observation.wheelPositions()[deltaIndex].positions[i].angle.getRadians();
-      //        lastTurnPositions[i] = lastWheelPositions.positions[i].angle.getRadians();
-      //        turnPositionDeltas[i] =
-      //            observation
-      //                .wheelPositions()[deltaIndex]
-      //                .positions[i]
-      //                .angle
-      //                .minus(lastWheelPositions.positions[i].angle)
-      //                .getRadians();
-      //      }
-      //      Logger.recordOutput("Odometry/TurnPositions", turnPositions);
-      //      Logger.recordOutput("Odometry/LastTurnPositions", lastTurnPositions);
-      //      Logger.recordOutput("Odometry/TurnPositionDeltas", turnPositionDeltas);
-      Twist2d twist =
-          kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions()[deltaIndex]);
-      lastWheelPositions = observation.wheelPositions()[deltaIndex];
-      // Set gyro angle to null if gyro not connected (sim)
-      if (observation.gyroConnected()) {
-        // Update dtheta for twist if gyro connected
-        twist =
-            new Twist2d(
-                twist.dx,
-                twist.dy,
-                observation.gyroAngles()[deltaIndex].minus(lastGyroAngle).getRadians());
-        lastGyroAngle = observation.gyroAngles()[deltaIndex];
-      }
-      // Add twist to odometry pose
-      odometryPose = odometryPose.exp(twist);
-      // Add pose to buffer at timestamp
-      poseBuffer.addSample(observation.timestamps()[deltaIndex], odometryPose);
-
-      // Calculate diff from last odom pose and add onto pose estimate
-      estimatedPose = estimatedPose.exp(lastOdometryPose.log(odometryPose));
-      // Set last odometry pose to current
-      lastOdometryPose = odometryPose;
+    // take minimum
+    Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions());
+    lastWheelPositions = observation.wheelPositions();
+    // Check gyro connected
+    if (observation.gyroAngle != null) {
+      // Update dtheta for twist if gyro connected
+      twist =
+          new Twist2d(
+              twist.dx, twist.dy, observation.gyroAngle().minus(lastGyroAngle).getRadians());
+      lastGyroAngle = observation.gyroAngle();
     }
+    // Add twist to odometry pose
+    odometryPose = odometryPose.exp(twist);
+    // Add pose to buffer at timestamp
+    poseBuffer.addSample(observation.timestamp(), odometryPose);
+    // Calculate diff from last odometry pose and add onto pose estimate
+    estimatedPose = estimatedPose.exp(lastOdometryPose.log(odometryPose));
   }
 
   public void addVisionObservation(VisionObservation observation) {

src/main/java/frc/robot/commands/DriveCommands.java

@@ -21,6 +21,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.RobotState;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveConstants;
 import java.util.function.DoubleSupplier;
@@ -64,7 +65,7 @@ public static Command joystickDrive(
                   linearVelocity.getX() * DriveConstants.drivetrainConfig.maxLinearVelocity(),
                   linearVelocity.getY() * DriveConstants.drivetrainConfig.maxLinearVelocity(),
                   omega * DriveConstants.drivetrainConfig.maxLinearVelocity(),
-                  drive.getGyroYaw()));
+                  RobotState.getInstance().getEstimatedPose().getRotation()));
         },
         drive);
   }

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -13,24 +13,36 @@
 
 package frc.robot.subsystems.drive;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.*;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotState;
 import java.util.*;
+import java.util.concurrent.ArrayBlockingQueue;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
+import java.util.stream.IntStream;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 public class Drive extends SubsystemBase {
   public static final Lock odometryLock = new ReentrantLock();
+  public static final Queue<Double> timestampQueue = new ArrayBlockingQueue<>(100);
+
   private final GyroIO gyroIO;
   private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
   private final ModuleIO[] modules = new ModuleIO[4]; // FL, FR, BL, BR
-  private final ModuleIOInputsAutoLogged[] moduleInputs = new ModuleIOInputsAutoLogged[4];
+  private final ModuleIOInputsAutoLogged[] moduleInputs =
+      new ModuleIOInputsAutoLogged[] {
+        new ModuleIOInputsAutoLogged(),
+        new ModuleIOInputsAutoLogged(),
+        new ModuleIOInputsAutoLogged(),
+        new ModuleIOInputsAutoLogged()
+      };
   private ChassisSpeeds robotVelocity = new ChassisSpeeds();
   private ChassisSpeeds fieldVelocity = new ChassisSpeeds();
 
@@ -45,62 +57,63 @@ public Drive(
     modules[1] = frModuleIO;
     modules[2] = blModuleIO;
     modules[3] = brModuleIO;
-    Arrays.fill(moduleInputs, new ModuleIOInputsAutoLogged());
   }
 
   public void periodic() {
     odometryLock.lock();
+    double[] timestamps = timestampQueue.stream().mapToDouble(Double::valueOf).toArray();
+    // Sim will not have any timstamps
+    if (timestamps.length == 0) {
+      timestamps = new double[] {Timer.getFPGATimestamp()};
+    }
+    timestampQueue.clear();
     gyroIO.updateInputs(gyroInputs);
     for (int i = 0; i < modules.length; i++) {
       modules[i].updateInputs(moduleInputs[i]);
     }
     odometryLock.unlock();
+
     Logger.processInputs("Drive/Gyro", gyroInputs);
     for (int i = 0; i < modules.length; i++) {
       Logger.processInputs("Drive/Module" + i, moduleInputs[i]);
       modules[i].periodic();
     }
 
+    // Calculate the min odometry position updates across all modules
+    int minOdometryUpdates =
+        IntStream.of(
+                timestamps.length,
+                Arrays.stream(moduleInputs)
+                    .mapToInt(
+                        moduleInput ->
+                            Math.min(
+                                moduleInput.odometryDrivePositionsMeters.length,
+                                moduleInput.odometryTurnPositions.length))
+                    .min()
+                    .orElse(0))
+            .min()
+            .orElse(0);
+    if (gyroInputs.connected) {
+      minOdometryUpdates = Math.min(gyroInputs.odometryYawPositions.length, minOdometryUpdates);
+    }
     // Pass odometry data to robot state
-    // Calculate the min odometry position count across all modules
-    OptionalInt minOdometryPositionsCount =
-        Arrays.stream(moduleInputs)
-            .mapToInt(
-                moduleInput ->
-                    Math.min(
-                        moduleInput.odometryDrivePositionsMeters.length,
-                        moduleInput.odometryTurnPositions.length))
-            .min();
-    // Add observation to robot state if a minimum odometry positions count is found
-    minOdometryPositionsCount.ifPresent(
-        minCount -> {
-          SwerveDriveWheelPositions[] wheelPositions = new SwerveDriveWheelPositions[minCount];
-          for (int i = 0; i < minCount; i++) {
-            int odometryPositionsIndex = i;
-            // Get all four swerve module positions at that odometry positions count
-            wheelPositions[i] =
-                new SwerveDriveWheelPositions(
-                    Arrays.stream(moduleInputs)
-                        .map(
-                            moduleInput ->
-                                new SwerveModulePosition(
-                                    moduleInput
-                                        .odometryDrivePositionsMeters[odometryPositionsIndex],
-                                    moduleInput.odometryTurnPositions[odometryPositionsIndex]))
-                        .toArray(SwerveModulePosition[]::new));
-          }
-          double[] timestamps = new double[minCount];
-          // Since timestamps are all synced we can use the timestamps from any module
-          System.arraycopy(moduleInputs[0].odometryTimestamps, 0, timestamps, 0, minCount);
-          // Add observation to robot state
-          RobotState.getInstance()
-              .addOdometryData(
-                  new RobotState.OdometryObservation(
-                      wheelPositions,
-                      gyroInputs.odometryYawPositions,
-                      timestamps,
-                      gyroInputs.connected));
-        });
+    for (int i = 0; i < minOdometryUpdates; i++) {
+      int odometryIndex = i;
+      Rotation2d yaw = gyroInputs.connected ? gyroInputs.odometryYawPositions[i] : null;
+      // Get all four swerve module positions at that odometry update
+      // and store in SwerveDriveWheelPositions object
+      SwerveDriveWheelPositions wheelPositions =
+          new SwerveDriveWheelPositions(
+              Arrays.stream(moduleInputs)
+                  .map(
+                      moduleInput ->
+                          new SwerveModulePosition(
+                              moduleInput.odometryDrivePositionsMeters[odometryIndex],
+                              moduleInput.odometryTurnPositions[odometryIndex]))
+                  .toArray(SwerveModulePosition[]::new));
+      RobotState.getInstance()
+          .addOdometryData(new RobotState.OdometryObservation(wheelPositions, yaw, timestamps[i]));
+    }
 
     // Stop moving when disabled
     if (DriverStation.isDisabled()) {

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -33,7 +33,7 @@ public final class DriveConstants {
         case COMPBOT -> 250.0;
       };
 
-  public static final Matrix<N3, N1> odometryStateStDevs =
+  public static final Matrix<N3, N1> odometryStateStdDevs =
       switch (Constants.getRobot()) {
         default -> new Matrix<>(VecBuilder.fill(0.1, 0.1, 0.1));
       };

src/main/java/frc/robot/subsystems/drive/ModuleIO.java

@@ -34,7 +34,6 @@ class ModuleIOInputs {
 
     public double[] odometryDrivePositionsMeters = new double[] {};
     public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};
-    public double[] odometryTimestamps = new double[] {};
   }
 
   /** Updates the set of loggable inputs. */

src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java

@@ -53,7 +53,6 @@ public class ModuleIOKrakenFOC implements ModuleIO {
   private final boolean isTurnMotorInverted = true;
 
   // queues
-  private final Queue<Double> timestampsQueue;
   private final Queue<Double> drivePositionQueue;
   private final Queue<Double> turnPositionQueue;
 
@@ -139,7 +138,6 @@ public ModuleIOKrakenFOC(ModuleConfig config) {
     turnCurrent = turnTalon.getStatorCurrent();
 
     // 250hz signals
-    timestampsQueue = PhoenixOdometryThread.getInstance().getTimestampQueue();
     drivePositionQueue =
         PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition());
     turnPositionQueue =
@@ -199,10 +197,8 @@ public void updateInputs(ModuleIOInputs inputs) {
         turnPositionQueue.stream()
             .map((signal) -> Rotation2d.fromRotations(signal / moduleConstants.turnReduction()))
             .toArray(Rotation2d[]::new);
-    inputs.odometryTimestamps = timestampsQueue.stream().mapToDouble(d -> d).toArray();
     drivePositionQueue.clear();
     turnPositionQueue.clear();
-    timestampsQueue.clear();
   }
 
   @Override

src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java

@@ -20,7 +20,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 
 /**
@@ -69,15 +68,15 @@ public void updateInputs(ModuleIOInputs inputs) {
 
     inputs.turnAbsolutePosition =
         new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition);
-    inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad());
+    inputs.turnPosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad());
     inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec();
     inputs.turnAppliedVolts = turnAppliedVolts;
     inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())};
 
     inputs.odometryDrivePositionsMeters =
         new double[] {driveSim.getAngularPositionRad() * wheelRadius};
-    inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition};
-    inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()};
+    inputs.odometryTurnPositions =
+        new Rotation2d[] {Rotation2d.fromRadians(turnSim.getAngularPositionRad())};
   }
 
   @Override

src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java

@@ -43,7 +43,6 @@ public class ModuleIOSparkMax implements ModuleIO {
   // Queues
   private final Queue<Double> drivePositionQueue;
   private final Queue<Double> turnPositionQueue;
-  private final Queue<Double> timestampQueue;
 
   private final Rotation2d absoluteEncoderOffset;
   private int currentIteration = reSeedIterations;
@@ -117,7 +116,6 @@ public ModuleIOSparkMax(ModuleConfig config) {
     turnPositionQueue =
         SparkMaxOdometryThread.getInstance()
             .registerSignal(() -> absoluteEncoderValue.get().getRadians());
-    timestampQueue = SparkMaxOdometryThread.getInstance().getTimestampQueue();
 
     driveFeedforward = new SimpleMotorFeedforward(moduleConstants.ffKs(), moduleConstants.ffKv());
     driveFeedback = new PIDController(moduleConstants.driveKp(), 0.0, moduleConstants.drivekD());
@@ -142,7 +140,6 @@ public void updateInputs(ModuleIOInputs inputs) {
         drivePositionQueue.stream().mapToDouble(rads -> rads * wheelRadius).toArray();
     inputs.odometryTurnPositions =
         turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new);
-    inputs.odometryTimestamps = timestampQueue.stream().mapToDouble(d -> d).toArray();
     drivePositionQueue.clear();
     turnPositionQueue.clear();
   }

src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java

@@ -35,7 +35,6 @@ public class PhoenixOdometryThread extends Thread {
       new ReentrantLock(); // Prevents conflicts when registering signals
   private BaseStatusSignal[] signals = new BaseStatusSignal[0];
   private final List<Queue<Double>> queues = new ArrayList<>();
-  private final Queue<Double> timestampQueue;
   private boolean isCANFD = false;
 
   private static PhoenixOdometryThread instance = null;
@@ -50,7 +49,6 @@ public static PhoenixOdometryThread getInstance() {
   private PhoenixOdometryThread() {
     setName("PhoenixOdometryThread");
     setDaemon(true);
-    timestampQueue = new ArrayDeque<>(100);
     start();
   }
 
@@ -97,14 +95,10 @@ public void run() {
         for (int i = 0; i < signals.length; i++) {
           queues.get(i).offer(signals[i].getValueAsDouble());
         }
-        timestampQueue.offer(fpgaTimestamp);
+        Drive.timestampQueue.offer(fpgaTimestamp);
       } finally {
         Drive.odometryLock.unlock();
       }
     }
   }
-
-  public Queue<Double> getTimestampQueue() {
-    return timestampQueue;
-  }
 }