Merge pull request #3 from FIRST1939/visionStandardDeviations

Vision Standard Deviations
FIRST1939 · Mar 9, 2024 · 0e95f5f · 0e95f5f
2 parents a92d937 + 60cdbba
commit 0e95f5f
Show file tree

Hide file tree

Showing 3 changed files with 66 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/commands/swerve/TrackAprilTags.java b/src/main/java/frc/robot/commands/swerve/TrackAprilTags.java
@@ -1,5 +1,11 @@
 package frc.robot.commands.swerve;
 
+import edu.wpi.first.math.Matrix;
+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;
@@ -22,9 +28,42 @@ public void execute () {
 
         if (!this.limelight.areValidMeasurements()) { return; }
 
+        Translation2d estimatedTranslation = this.swerve.getPoseEstimator().getEstimatedPosition().getTranslation();
+        double poseDifference = estimatedTranslation.getDistance(this.limelight.getLatestPose().getTranslation());
+
+        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(
             this.limelight.getLatestPose(), 
-            Timer.getFPGATimestamp() - (this.limelight.getLatestDelay() / 1000.0)
+            Timer.getFPGATimestamp() - (this.limelight.getLatestDelay() / 1000.0),
+            standardDeviations
         );
     }
 

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -1,20 +1,21 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.math.geometry.Pose2d;
-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 int latestTargets;
+    private double latestTargetArea;
 
     private boolean validMeasurements;
     private Timer usageTimer = new Timer();
@@ -39,12 +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();
+
+            this.latestTargets = targetingResults.targets_Fiducials.length;
+
+            double bestTargetArea = 0.0;
+
+            for (LimelightTarget_Fiducial aprilTag : targetingResults.targets_Fiducials) {
+
+                if (aprilTag.ta > bestTargetArea) { 
+
+                    bestTargetArea = aprilTag.ta; 
+                }
+            }
+
+            this.latestTargetArea = bestTargetArea;
         }
 
         double lastUpdated = this.usageTimer.get();
@@ -56,5 +68,7 @@ public void periodic () {
 
     public Pose2d getLatestPose () { return this.latestPose; }
     public double getLatestDelay () { return this.latestDelay; }
+    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
@@ -9,10 +9,14 @@
 import com.pathplanner.lib.util.PathPlannerLogging;
 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;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -140,6 +144,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(); }
 
@@ -162,7 +167,7 @@ public void zeroGyro () {
 
     public void resetOdometry (Pose2d pose) { this.swerveDrive.resetOdometry(pose); }
     public void setChassisSpeeds (ChassisSpeeds chassisSpeeds) { this.swerveDrive.drive(chassisSpeeds); }
-    public void addVisionMeasurement (Pose2d pose2d, double timestamp) { this.swerveDrive.addVisionMeasurement(pose2d, timestamp); }
+    public void addVisionMeasurement (Pose2d pose2d, double timestamp, Matrix<N3, N1> standardDeviations) { this.swerveDrive.addVisionMeasurement(pose2d, timestamp, standardDeviations); }
 
     public void lock () { this.swerveDrive.lockPose(); }
     public void setBrakeMode (boolean brake) { this.swerveDrive.setMotorIdleMode(brake); }