did some pose stuff (vision)

Team334 · Mar 28, 2024 · fabb36d · fabb36d
1 parent d8d6319
commit fabb36d
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 6 deletions.
diff --git 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
@@ -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;