switch to counting tags

team401 · Dec 3, 2024 · c26ee5c · c26ee5c
1 parent 28a704a
commit c26ee5c
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 8 deletions.
diff --git a/vision/src/main/java/coppercore/vision/Camera.java b/vision/src/main/java/coppercore/vision/Camera.java
@@ -46,13 +46,18 @@ public Matrix<N3, N1> getLatestVariance() {
         if (numTags == 0) {
             return stdDev;
         } else if (numTags > 1) {
-                stdDev = CoreVisionConstants.multiTagStdDev;
-        } else if (numTags == 1 && avgDistanceFromTarget > CoreVisionConstants.singleTagDistanceCutoff) {
-                return CoreVisionConstants.rejectionStdDev;
+            stdDev = CoreVisionConstants.multiTagStdDev;
+        } else if (numTags == 1
+                && avgDistanceFromTarget > CoreVisionConstants.singleTagDistanceCutoff) {
+            return CoreVisionConstants.rejectionStdDev;
         }
 
         // distance based variance
-        stdDev = stdDev.times(1 + (Math.pow(avgDistanceFromTarget, 2) / CoreVisionConstants.distanceFactor));
+        stdDev =
+                stdDev.times(
+                        1
+                                + (Math.pow(avgDistanceFromTarget, 2)
+                                        / CoreVisionConstants.distanceFactor));
 
         return stdDev;
     }

diff --git a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java
@@ -73,11 +73,10 @@ public void updateInputs(CameraIOInputs inputs) {
 
         photonPose.ifPresentOrElse(
                 (pose) -> {
+                    calculateAverageTagDistance(pose, inputs);
                     inputs.latestFieldToRobot = pose.estimatedPose;
-                    inputs.nTags = pose.targetsUsed.size();
 
                     inputs.latestTimestampSeconds = this.latestTimestampSeconds;
-                    inputs.averageTagDistanceM = calculateAverageTagDistance(pose);
                     inputs.averageTagYaw = calculateAverageTagYaw(pose);
 
                     inputs.wasAccepted = true;
@@ -98,13 +97,15 @@ public void updateInputs(CameraIOInputs inputs) {
     //     return visionEstimate;
     // }
 
-    private double calculateAverageTagDistance(EstimatedRobotPose pose) {
+    private void calculateAverageTagDistance(EstimatedRobotPose pose, CameraIOInputs inputs) {
         double distance = 0.0;
+        int numTags = 0;
         for (PhotonTrackedTarget target : pose.targetsUsed) {
             var tagPose = poseEstimator.getFieldTags().getTagPose(target.getFiducialId());
             if (tagPose.isEmpty()) {
                 continue;
             }
+            numTags += 1;
             distance +=
                     tagPose.get()
                             .toPose2d()
@@ -113,7 +114,8 @@ private double calculateAverageTagDistance(EstimatedRobotPose pose) {
         }
         distance /= pose.targetsUsed.size();
 
-        return distance;
+        inputs.nTags = numTags;
+        inputs.averageTagDistanceM = distance;
     }
 
     private static Rotation2d calculateAverageTagYaw(EstimatedRobotPose pose) {