From 76809b51bcaf1f22f622919fe1028a32e04186d3 Mon Sep 17 00:00:00 2001
From: Jack Lingle <captainjack0818@gmail.com>
Date: Wed, 7 Feb 2024 18:54:19 -0500
Subject: [PATCH] make single tag uncertainty greater

---
 src/main/java/frc/robot/Constants.java                      | 6 ++----
 src/main/java/frc/robot/subsystems/Navigation.java          | 6 +++++-
 src/main/java/frc/robot/subsystems/vision/VisionIOReal.java | 2 +-
 .../frc/robot/subsystems/vision/VisionIOSimulation.java     | 3 +--
 4 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 33f8ac5..8416f94 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -77,12 +77,11 @@ public static class VisionConstants { // ALL PLACEHOLDERS
         public static final int CAMERA_FPS = 20;
         public static final double IDEAL_GOAL_RANGE_METERS = 1;
 
-
-         // 0.45 from 2023
+        // 0.45 from 2023
         public static final Matrix<N3, N1> lowCameraUncertainty = VecBuilder.fill(1.2, 1.2, 2);
         // 1.2 from 2023
         public static final Matrix<N3, N1> highCameraUncertainty = VecBuilder.fill(3.5, 3.5, 10);
-        public static final Matrix<N3, N1> singleTagUncertainty = VecBuilder.fill(20.0, 20.0, 10);
+        public static final Matrix<N3, N1> singleTagUncertainty = VecBuilder.fill(20.0, 20.0, 20.0);
 
         // simulation
         public static final int RESOLUTION_WIDTH = 480;
@@ -102,7 +101,6 @@ public static class FieldConstants {
         public static final AprilTagFieldLayout FIELD_LAYOUT = initLayout();
         public static final double midfieldLowThresholdM = 5.87;
         public static final double midfieldHighThresholdM = 10.72;
-
     }
 
     private static AprilTagFieldLayout initLayout() {
diff --git a/src/main/java/frc/robot/subsystems/Navigation.java b/src/main/java/frc/robot/subsystems/Navigation.java
index 960f050..8e5208a 100644
--- a/src/main/java/frc/robot/subsystems/Navigation.java
+++ b/src/main/java/frc/robot/subsystems/Navigation.java
@@ -85,6 +85,7 @@ private boolean robotInMidField() {
         return currentPose.getX() > FieldConstants.midfieldLowThresholdM
                 && currentPose.getX() < FieldConstants.midfieldHighThresholdM;
     }
+
     private Matrix<N3, N1> cameraUncertainty(double averageTagDistanceM, int nTags) {
         /*
          * On this year's field, AprilTags are arranged into rough 'corridors' between the stage and
@@ -107,7 +108,10 @@ public void updateOdometry(
 
     public void updateNavVision() {
         if (inputs.poseAvailable && inputs.newResult) {
-            poseEstimator.addVisionMeasurement(inputs.estimatedVisionPose, inputs.timestampSeconds, cameraUncertainty(inputs.averageTagDistanceM, inputs.nTags));
+            poseEstimator.addVisionMeasurement(
+                    inputs.estimatedVisionPose,
+                    inputs.timestampSeconds,
+                    cameraUncertainty(inputs.averageTagDistanceM, inputs.nTags));
         }
     }
 
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java
index bce12d7..8fde0c1 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java
@@ -5,7 +5,6 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import frc.robot.Constants;
-
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
@@ -66,6 +65,7 @@ public void updateInputs(VisionIOInputs inputs) {
     public void updatePose(Pose2d drivetrainPoseMeters) {
         cameraPoseEstimator.setReferencePose(drivetrainPoseMeters);
     }
+
     private static double calculateAverageTagDistance(EstimatedRobotPose pose) {
         double distance = 0.0;
         for (PhotonTrackedTarget target : pose.targetsUsed) {
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSimulation.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSimulation.java
index 6eff604..486f3c8 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOSimulation.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOSimulation.java
@@ -7,7 +7,6 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import frc.robot.Constants;
-
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
@@ -95,7 +94,7 @@ public void updatePose(Pose2d simRobotPoseMeters) {
     public void set3dFieldSimActive(boolean enabled) {
         simulatedCamera.enableDrawWireframe(enabled);
     }
-    
+
     private static double calculateAverageTagDistance(EstimatedRobotPose pose) {
         double distance = 0.0;
         for (PhotonTrackedTarget target : pose.targetsUsed) {