fix: accept tags when disabled

flamingchickens1540 · Oct 19, 2024 · 9c6b5f7 · 9c6b5f7
1 parent e1fddf2
commit 9c6b5f7
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
@@ -359,7 +359,7 @@ public void addVisionMeasurement(EstimatedVisionPose visionPose) {
             Matrix<N3, N1> stdDevs = visionPose.getStdDevs();
             visionPoseEstimator.setVisionMeasurementStdDevs(stdDevs);
             visionPoseEstimator.addVisionMeasurement(visionPose.poseMeters.toPose2d(), visionPose.timestampSecs);
-            if (!blockTags && (!RobotState.isAutonomous() || getSpeakerDistanceMeters() < 4.5)) {
+            if (!blockTags && (!RobotState.isAutonomous() || RobotState.isDisabled() || getSpeakerDistanceMeters() < 4.5)) {
                 poseEstimator.setVisionMeasurementStdDevs(stdDevs);
                 poseEstimator.addVisionMeasurement(visionPose.poseMeters.toPose2d(), visionPose.timestampSecs);
             }