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 currentPose, Field2d field) */ public void updatePoseEstimation(SwerveDrive swerveDrive) { - for (EstimatedRobotPose i : getEstimatedGlobalPose()) - { - swerveDrive.addVisionMeasurement(i.estimatedPose.toPose2d(), i.timestampSeconds); - } + ArrayList estimatedRobotPoses = getEstimatedGlobalPose(); + if(Robot.isReal()) { + for (EstimatedRobotPose i : estimatedRobotPoses) + { + swerveDrive.addVisionMeasurement(i.estimatedPose.toPose2d(), i.timestampSeconds); + } + } else visionSim.update(swerveDrive.getPose()); } /**