[Robot] Remove unneeded vision update sort

SouthwestRoboticsProgramming · Nov 20, 2023 · 6c0e225 · 6c0e225
1 parent 4e43fcd
commit 6c0e225
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java
@@ -101,8 +101,11 @@ public void update(Twist2d driveTwist) {
 
                 List<TagTrackerInput.VisionUpdate> newVisionUpdates = new ArrayList<>();
                 newVisionUpdates.add(visionUpdate);
-                newVisionUpdates.sort(this::compareStdDevs);
+
+                // Insert new update entry for this vision update
                 updates.put(timestamp, new PoseUpdate(prevToVisionTwist, newVisionUpdates));
+
+                // Overwrite nextUpdate with twist after this vision update
                 updates.put(nextUpdate.getKey(), new PoseUpdate(visionToNextTwist, nextUpdate.getValue().visionUpdates));
             }
         }