Ignore vision measurements if less than two tags are detected

Resolves #18
CurtinFRC · Jul 3, 2024 · ada0c7d · ada0c7d
1 parent 00aaa77
commit ada0c7d
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
@@ -113,8 +113,10 @@ public void periodic() {
               new Translation3d(data[0], data[1], data[2]),
               new Rotation3d(data[4], data[5], data[6]));
 
-      if (reasonablePose(m_botpose)) {
-        addVisionMeasurement(m_botpose.toPose2d(), frc.robot.Utils.now());
+      if ((int) data[7] < 2) {
+        if (reasonablePose(m_botpose)) {
+          addVisionMeasurement(m_botpose.toPose2d(), frc.robot.Utils.now());
+        }
       }
     }
   }