From f2224bcd3f7894710dc0ae0f191cd3d94dab645d Mon Sep 17 00:00:00 2001
From: Brandonzx3 <btherr2003@gmail.com>
Date: Thu, 29 Aug 2024 00:09:48 -0500
Subject: [PATCH] update the vision simulation on Vision#updatePoseEstimation
 and check for real env before adding vision measurements

---
 .../java/frc/robot/subsystems/swervedrive/Vision.java | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java
index 9b821591..b65d9d34 100644
--- 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());
   }
 
   /**