Vision tweaks

src/main/java/com/team1816/lib/subsystems/Vision.java

@@ -47,7 +47,8 @@ public class Vision extends SubsystemBase implements ITestableSubsystem {
     /**
      * The base standard deviations to use for non-multi-tag estimates
      */
-    private final Matrix<N3, N1> singleTagStdDevs = VecBuilder.fill(4, 4, 8);
+    // Ignore single tag data. Previously set to 4, 4, 8.
+    private final Matrix<N3, N1> singleTagStdDevs = VecBuilder.fill(999999, 999999, 999999);
     /**
      * The base standard deviations to use for multi-tag estimates
      */
@@ -58,6 +59,11 @@ public class Vision extends SubsystemBase implements ITestableSubsystem {
     private final Matrix<N3, N1> noTrustStdDevs = VecBuilder.fill(
         Double.NaN, Double.NaN, Double.NaN
     );
+    /**
+     * The number of vision pose estimates we have discarded in a row because they were too far off
+     * from the combined pose estimate.
+     */
+    private int consecutiveDiscardedEstimates = 0;
 
     /**
      * Constructs a Vision subsystem.
@@ -89,7 +95,7 @@ public List<Pair<EstimatedRobotPose, Matrix<N3, N1>>> getVisionEstimatedPosesWit
 
         // The maximum distance a vision pose estimate can be from the current robot pose
         // estimate to allow the vision estimate to be used.
-        double visionEstimateDistanceThresholdMeters = 1.0;
+        double visionEstimateDistanceThresholdMeters = 1.5;
         // The maximum difference the angle of a vision pose estimate can be from the angle
         // of the current robot pose estimate to allow the vision estimate to be used.
         double visionEstimateAngleThresholdRadians = Units.degreesToRadians(15.0);
@@ -103,7 +109,6 @@ public List<Pair<EstimatedRobotPose, Matrix<N3, N1>>> getVisionEstimatedPosesWit
                 // out unreasonable estimates caused by pose ambiguity (see here:
                 // https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/3D-tracking.html#ambiguity
                 // ).
-
                 if (
                     // If we don't currently have an accurate pose estimate, we can't use current
                     // pose estimate to throw out far off vision estimates, so we'll just add the
@@ -128,6 +133,10 @@ public List<Pair<EstimatedRobotPose, Matrix<N3, N1>>> getVisionEstimatedPosesWit
                                 ) < visionEstimateAngleThresholdRadians
                         )
                 ) {
+                    // We didn't discard this estimate for being too far off from the combined
+                    // estimate, so reset the counter.
+                    consecutiveDiscardedEstimates = 0;
+                    // Calculate the standard deviations for the estimate.
                     Matrix<N3, N1> standardDeviations = calculateEstimateStandardDeviations(estimatedRobotPose);
                     // If the standard deviations are the noTrustStdDevs, we'll just throw the
                     // estimate out. Otherwise, add the estimate to the list to return.
@@ -138,6 +147,20 @@ public List<Pair<EstimatedRobotPose, Matrix<N3, N1>>> getVisionEstimatedPosesWit
                     // for logging purposes.
                     camera.latestVisionStdDevs = standardDeviations;
                 }
+                // If we discarded enough estimates in a row because they were too far off from the
+                // current pose estimate, then it is probably because something is wrong with the
+                // current pose estimate (likely due to wheel slippage from hitting something). If
+                // this is the case, say that we don't trust the current estimate to allow vision
+                // to fully recorrect.
+                else {
+                    consecutiveDiscardedEstimates ++;
+                    // The number of estimates to allow to be discarded before determining that we
+                    // have lost a good pose estimate.
+                    int discardsBeforePoseLoss = 5;
+                    if (consecutiveDiscardedEstimates >= discardsBeforePoseLoss) {
+                        BaseRobotState.hasAccuratePoseEstimate = false;
+                    }
+                }
             }
         }
 

src/main/resources/yaml/ztldr.yml

@@ -178,7 +178,7 @@ subsystems:
         constants:
             feedingDutyCycle: 0.7
             stoppedDutyCycle: 0
-            agitateForwardDutyCycle: 0.15
+            agitateForwardDutyCycle: -0.15
             agitateStoppedDutyCycle: 0
     intake:
         implemented: true
@@ -329,7 +329,7 @@ subsystems:
                     yInches: 12.1
                     zInches: 18
                     rollDegrees: 0
-                    pitchDegrees: 0
+                    pitchDegrees: -8
                     yawDegrees: 55
                 detectionType: APRIL_TAG
             forwardRight: # Pi IP: 10.18.16.12.
@@ -339,7 +339,7 @@ subsystems:
                     yInches: -12.1
                     zInches: 18
                     rollDegrees: 0
-                    pitchDegrees: 0
+                    pitchDegrees: -8
                     yawDegrees: -55
                 detectionType: APRIL_TAG
             backwardLeft: # Pi IP: 10.18.16.13
@@ -349,7 +349,7 @@ subsystems:
                     yInches: 12.1
                     zInches: 16
                     rollDegrees: 0
-                    pitchDegrees: 0
+                    pitchDegrees: -8
                     yawDegrees: 155
                 detectionType: APRIL_TAG
             backwardRight: # Pi IP: 10.18.16.14
@@ -359,7 +359,7 @@ subsystems:
                     yInches: -12.1
                     zInches: 16
                     rollDegrees: 0
-                    pitchDegrees: 0
+                    pitchDegrees: -8
                     yawDegrees: -155
                 detectionType: APRIL_TAG
             fuelDetector: