From fabb36d0e39a4636056b4e003993df55225118a2 Mon Sep 17 00:00:00 2001
From: Jerry <zeruiz@nycstudents.net>
Date: Thu, 28 Mar 2024 17:05:49 -0400
Subject: [PATCH] did some pose stuff (vision)

---
 src/main/java/frc/robot/Constants.java             |  1 +
 .../frc/robot/subsystems/SwerveDriveSubsystem.java | 14 ++++++++------
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index afe5105..9fbc6d2 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -184,6 +184,7 @@ public static class FieldConstants {
     public static final double SHOOTER_SLOW_THRESHOLD = 2;
 
     public static final double TAG_DISTANCE_THRESHOLD = 3.5;
+    public static final double SINGLE_TAG_DISTANCE_THRESHOLD = 2;
 
     public static final int SPEAKER_TAG_BLUE = 7;
     public static final int SPEAKER_TAG_RED = 4;
diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
index 6e7f153..801b04a 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -207,6 +207,7 @@ private void updateBotpose() {
       double xyStds;
       double yawStd = 9999999;
 
+      //  bad distance, ignore vision completely
       if (tagDistance > FieldConstants.TAG_DISTANCE_THRESHOLD) {
         return;
       }
@@ -216,14 +217,15 @@ private void updateBotpose() {
         xyStds = 0.55;
       }
 
-      // good distance
-      else if (tagDistance > 0.8 && poseDifference >= 0.5) {
-        xyStds = 0.65;
+      // one tag, closer distance, estimated pose is accurate
+      else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDifference >= 0.3) {
+        xyStds = 0.85;
       }
 
-      // else if (tagDistance > 0.3 && poseDifference >= 0.3) {
-
-      // }
+      // one tag, closer distance, estimated pose is inaccurate
+      else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDifference >= 0.5) {
+        xyStds = 0.65;
+      }
 
       else {
         return;