Official Limelight Standard Deviation Values

FIRST1939 · Feb 29, 2024 · 60cdbba · 60cdbba
1 parent 32e64c1
commit 60cdbba
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 38 deletions.
diff --git a/src/main/java/frc/robot/commands/swerve/TrackAprilTags.java b/src/main/java/frc/robot/commands/swerve/TrackAprilTags.java
@@ -1,15 +1,15 @@
 package frc.robot.commands.swerve;
 
-import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Limelight;
 import frc.robot.subsystems.Swerve;
-import frc.robot.util.Constants;
 
 public class TrackAprilTags extends Command {
 
@@ -28,14 +28,36 @@ public void execute () {
 
         if (!this.limelight.areValidMeasurements()) { return; }
 
-        double distanceToTag = this.limelight.getLatestDistance().getNorm();
-        double distanceToTag2 = distanceToTag * distanceToTag;
+        Translation2d estimatedTranslation = this.swerve.getPoseEstimator().getEstimatedPosition().getTranslation();
+        double poseDifference = estimatedTranslation.getDistance(this.limelight.getLatestPose().getTranslation());
 
-        Matrix<N3, N1> standardDeviations = MatBuilder.fill(
-            Nat.N3(), Nat.N1(), 
-            Constants.SwerveConstants.LIMELIGHT_DEFAULT_DEVIATIONS.get(0, 0) * distanceToTag2,
-            Constants.SwerveConstants.LIMELIGHT_DEFAULT_DEVIATIONS.get(1, 0) * distanceToTag2,
-            Constants.SwerveConstants.LIMELIGHT_DEFAULT_DEVIATIONS.get(2, 0) * distanceToTag2
+        double xyStandardDeviation = 0.0;
+        double rotationStandardDeviation = 0.0;
+
+        if (this.limelight.getLatestTargets() >= 2) {
+
+            // Multiple Targets Detected
+            xyStandardDeviation = 0.5;
+            rotationStandardDeviation = 6.0;
+        } else if (this.limelight.getLatestTargetArea() > 0.8 && poseDifference < 0.5) {
+
+            // 1 Target with Large Area and Close to Estimated Pose
+            xyStandardDeviation = 1.0;
+            rotationStandardDeviation = 12.0;
+        } else if (this.limelight.getLatestTargetArea() > 0.1 && poseDifference < 0.3) {
+
+            // 1 Target Farther Away but Close to Estimated Pose
+            xyStandardDeviation = 2.0;
+            rotationStandardDeviation = 30.0;
+        } else { 
+
+            // Insufficient Targeting Data
+            return; 
+        }
+
+        Matrix<N3, N1> standardDeviations = VecBuilder.fill(
+            xyStandardDeviation, xyStandardDeviation, 
+            Units.degreesToRadians(rotationStandardDeviation)
         );
 
         this.swerve.addVisionMeasurement(

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -1,22 +1,21 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.lib.LimelightHelpers;
 import frc.lib.LimelightHelpers.LimelightResults;
+import frc.lib.LimelightHelpers.LimelightTarget_Fiducial;
 import frc.lib.LimelightHelpers.Results;
 import frc.robot.util.Alerts;
 
 public class Limelight extends SubsystemBase {
 
     private Pose2d latestPose;
     private double latestDelay;
-    private Translation3d latestDistance;
+    private int latestTargets;
+    private double latestTargetArea;
 
     private boolean validMeasurements;
     private Timer usageTimer = new Timer();
@@ -41,31 +40,23 @@ public void periodic () {
 
         if (this.validMeasurements) {
 
-            if (!DriverStation.getAlliance().isPresent()) { this.latestPose = targetingResults.getBotPose2d(); } 
-            else if (DriverStation.getAlliance().get() == Alliance.Red) { this.latestPose = targetingResults.getBotPose2d_wpiRed(); } 
-            else { this.latestPose = targetingResults.getBotPose2d_wpiBlue(); }
-
+            this.latestPose = targetingResults.getBotPose2d_wpiBlue();
             this.latestDelay = targetingResults.latency_capture + targetingResults.latency_pipeline;
             this.usageTimer.restart();
 
-            Translation3d[] tagDistances = new Translation3d[targetingResults.targets_Fiducials.length];
-            for (int i = 0; i < targetingResults.targets_Fiducials.length; i++) { tagDistances[i] = targetingResults.targets_Fiducials[i].getTargetPose_CameraSpace().getTranslation(); }
-
-            double tagDistanceX, tagDistanceY, tagDistanceZ;
-            tagDistanceX = tagDistanceY = tagDistanceZ = 0.0;
+            this.latestTargets = targetingResults.targets_Fiducials.length;
+
+            double bestTargetArea = 0.0;
 
-            for (Translation3d tagDistance : tagDistances) {
+            for (LimelightTarget_Fiducial aprilTag : targetingResults.targets_Fiducials) {
 
-                tagDistanceX += tagDistance.getX();
-                tagDistanceY += tagDistance.getY();
-                tagDistanceZ += tagDistance.getZ();
+                if (aprilTag.ta > bestTargetArea) { 
+
+                    bestTargetArea = aprilTag.ta; 
+                }
             }
 
-            tagDistanceX /= tagDistances.length;
-            tagDistanceY /= tagDistances.length;
-            tagDistanceZ /= tagDistances.length;
-
-            this.latestDistance = new Translation3d(tagDistanceX, tagDistanceY, tagDistanceZ);
+            this.latestTargetArea = bestTargetArea;
         }
 
         double lastUpdated = this.usageTimer.get();
@@ -77,6 +68,7 @@ public void periodic () {
 
     public Pose2d getLatestPose () { return this.latestPose; }
     public double getLatestDelay () { return this.latestDelay; }
-    public Translation3d getLatestDistance () { return this.latestDistance; }
+    public int getLatestTargets () { return this.latestTargets; }
+    public double getLatestTargetArea () { return this.latestTargetArea; }
     public boolean areValidMeasurements () { return this.validMeasurements; }
 }
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -10,6 +10,7 @@
 import com.pathplanner.lib.util.ReplanningConfig;
 
 import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -142,6 +143,7 @@ public void driveHeadingLock (Translation2d translation, Rotation2d rotation) {
         );
     }
 
+    public SwerveDrivePoseEstimator getPoseEstimator () { return this.swerveDrive.swerveDrivePoseEstimator; }
     public Rotation2d getHeading () { return this.swerveDrive.getYaw(); }
     public Pose2d getPose () { return this.swerveDrive.getPose(); }
 

diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java
@@ -2,10 +2,7 @@
 
 import java.util.List;
 
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import swervelib.math.Matter;
@@ -44,8 +41,6 @@ public final class SwerveConstants {
         public static final double REPLANNING_TOTAL_ERROR = 0.15; // Total Error to Replan Path [m]
         public static final double REPLANNING_ERROR_SPIKE = 0.1; // Spike in Error to Replan Path [m / 20ms]
 
-        public static final Vector<N3> LIMELIGHT_DEFAULT_DEVIATIONS = VecBuilder.fill(0.3, 0.3, 0.3);
-
         public static final Config DRIVE_SYSID_CONFIG = new Config(
             Units.Volts.of(1.5).per(Units.Second),
             Units.Volts.of(9.6),