make single tag uncertainty greater

team401 · Feb 7, 2024 · 76809b5 · 76809b5
1 parent adf2cc7
commit 76809b5
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 8 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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) {