diff --git a/src/main/java/com/team1816/lib/subsystems/Vision.java b/src/main/java/com/team1816/lib/subsystems/Vision.java index 0c397f63..23c62f98 100644 --- a/src/main/java/com/team1816/lib/subsystems/Vision.java +++ b/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 singleTagStdDevs = VecBuilder.fill(4, 4, 8); + // Ignore single tag data. Previously set to 4, 4, 8. + private final Matrix 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 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>> 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>> 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>> 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 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>> 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; + } + } } } diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index eda9f7a0..e3088ee9 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/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: