Merge pull request #224 from brandonzx3/dev

Update the vision simulation on Vision#updatePoseEstimation
BroncBotz3481 · Aug 29, 2024 · b215996 · b215996
2 parents c7970ca + f2224bc
commit b215996
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java
@@ -209,10 +209,13 @@ public Vision(Supplier<Pose2d> currentPose, Field2d field)
    */
   public void updatePoseEstimation(SwerveDrive swerveDrive)
   {
-    for (EstimatedRobotPose i : getEstimatedGlobalPose())
-    {
-      swerveDrive.addVisionMeasurement(i.estimatedPose.toPose2d(), i.timestampSeconds);
-    }
+    ArrayList<EstimatedRobotPose> estimatedRobotPoses = getEstimatedGlobalPose();
+    if(Robot.isReal()) {
+      for (EstimatedRobotPose i : estimatedRobotPoses)
+      {
+        swerveDrive.addVisionMeasurement(i.estimatedPose.toPose2d(), i.timestampSeconds);
+      }
+    } else visionSim.update(swerveDrive.getPose());
   }
 
   /**