Fix high freq odometry bug

Galaxia5987 · Feb 13, 2024 · 001e565 · 001e565
1 parent 273b361
commit 001e565
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/swerve/SwerveDrive.java b/src/main/java/frc/robot/swerve/SwerveDrive.java
@@ -147,6 +147,7 @@ public double[] getHighFreqTimeStamps() {
     }
 
     public void updateHighFreqPose() {
+        highFreqModulePositions.clear();
         int sampleCount = Integer.MAX_VALUE;
         for (int i = 0; i < 4; i++) {
             sampleCount = Math.min(sampleCount, modules[i].getHighFreqAngles().length);
@@ -162,8 +163,7 @@ public void updateHighFreqPose() {
                                 modules[moduleIndex].getHighFreqDriveDistances()[sampleIndex],
                                 Rotation2d.fromRadians(
                                         modules[moduleIndex].getHighFreqAngles()[sampleIndex]));
-                highFreqModulePositions.get(sampleIndex)[moduleIndex] =
-                        tempHighFreqModulePositions[moduleIndex];
+                highFreqModulePositions.add(tempHighFreqModulePositions);
             }
             poseEstimator.updatePose();
             botPose = poseEstimator.getEstimatedPose();