From 0e9aa0fc77f9352abf060052fd30cd2bf9a1c094 Mon Sep 17 00:00:00 2001
From: Jade Turner <spacey-sooty@proton.me>
Date: Wed, 26 Jun 2024 13:17:56 +0800
Subject: [PATCH] Ignore vision measurements if less than two tags are detected

Resolves https://github.com/CurtinFRC/2024-Offseason/issues/18
---
 .../java/frc/robot/subsystems/CommandSwerveDrivetrain.java  | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
index 249042a..c901e1c 100644
--- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
@@ -112,8 +112,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());
+        }
       }
     }
   }