From 89c99d192cfd0e3752a9941c6275d8f6de0fe1fb Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Thu, 28 Mar 2024 11:04:57 -0400
Subject: [PATCH] working out limelight pose estimator

---
 src/main/java/frc/robot/Constants.java        |  3 +-
 .../subsystems/SwerveDriveSubsystem.java      | 29 ++++++++++++++-----
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c78b57c..d54d25e 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -183,8 +183,7 @@ public static class FieldConstants {
 
     public static final double SHOOTER_SLOW_THRESHOLD = 2;
 
-    public static final double CLOSE_TAG_DISTANCE_THRESHOLD = 3;
-    public static final double FAR_TAG_DISTANCE_THRESHOLD = 4;
+    public static final double TAG_DISTANCE_THRESHOLD = 3.5;
 
     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 a49956d..6e7f153 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -4,6 +4,7 @@
 import java.util.Optional;
 
 import com.ctre.phoenix6.Orchestra;
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric;
 import com.kauailabs.navx.frc.AHRS;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
@@ -193,24 +194,36 @@ private void updateBotpose() {
     // UPDATE BOTPOSE WITH VISION
     if (visionBotpose.isPresent()) {
       SmartDashboard.putBoolean("SEES TAG", true);
-      double[] botpose = visionBotpose.get();
+      double[] llBotpose = visionBotpose.get();
 
-      double tagDistance = botpose[9];
+      double tagDistance = llBotpose[9];
+      double tagCount = llBotpose[7];
+
+      Pose2d botpose = UtilFuncs.ToPose(llBotpose);
+      double poseDifference = botpose.getTranslation().getDistance(botpose.getTranslation());
 
       SmartDashboard.putNumber("DISTANCE TAGG", tagDistance);
 
       double xyStds;
       double yawStd = 9999999;
 
+      if (tagDistance > FieldConstants.TAG_DISTANCE_THRESHOLD) {
+        return;
+      }
+
+      // good distance, multiple tags
+      if (tagCount >= 2) {
+        xyStds = 0.55;
+      }
+
       // good distance
-      if (tagDistance <= FieldConstants.CLOSE_TAG_DISTANCE_THRESHOLD) {
+      else if (tagDistance > 0.8 && poseDifference >= 0.5) {
         xyStds = 0.65;
       }
 
-      // mid distance
-      else if (tagDistance <= FieldConstants.FAR_TAG_DISTANCE_THRESHOLD) {
-        xyStds = 0.95;
-      }
+      // else if (tagDistance > 0.3 && poseDifference >= 0.3) {
+
+      // }
 
       else {
         return;
@@ -219,7 +232,7 @@ else if (tagDistance <= FieldConstants.FAR_TAG_DISTANCE_THRESHOLD) {
       SmartDashboard.putNumber("STD", xyStds);
       System.out.println("UPDATING POSE");
 
-      _estimator.addVisionMeasurement(UtilFuncs.ToPose(botpose), _visionSubsystem.getLatency(), VecBuilder.fill(xyStds, xyStds, yawStd));
+      _estimator.addVisionMeasurement(UtilFuncs.ToPose(llBotpose), _visionSubsystem.getLatency(), VecBuilder.fill(xyStds, xyStds, yawStd));
     }
   }