From ddfac5741848c3354319af14934ab3ecb84344e2 Mon Sep 17 00:00:00 2001 From: henrymilbert <110063972+henrymilbert@users.noreply.github.com> Date: Thu, 2 Apr 2026 21:44:47 -0500 Subject: [PATCH 1/3] remove vision distance and angle cutoff, throw out single tag estimates, and reverse agitator direction --- .../com/team1816/lib/subsystems/Vision.java | 50 ++++++++++--------- src/main/resources/yaml/ztldr.yml | 2 +- 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/src/main/java/com/team1816/lib/subsystems/Vision.java b/src/main/java/com/team1816/lib/subsystems/Vision.java index 0c397f63..fe09e901 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 */ @@ -89,7 +90,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); @@ -105,28 +106,29 @@ public List>> getVisionEstimatedPosesWit // ). 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 - // vision estimate to the list no matter where it is. - !BaseRobotState.hasAccuratePoseEstimate - || ( - // Check if the vision estimate is within the distance threshold of the - // current pose estimate. - visionEstimatedPose2d.getTranslation().getDistance( - BaseRobotState.robotPose.getTranslation() - ) < visionEstimateDistanceThresholdMeters - // Check if the vision estimate is within the angle threshold of - // the current pose estimate. Get the absolute value of the - // difference between the angles constrained from -pi radians to pi - // radians to find the positive shortest difference. - && Math.abs( - MathUtil.angleModulus( - visionEstimatedPose2d.getRotation() - .minus(BaseRobotState.robotPose.getRotation()) - .getRadians() - ) - ) < visionEstimateAngleThresholdRadians - ) + true +// // 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 +// // vision estimate to the list no matter where it is. +// !BaseRobotState.hasAccuratePoseEstimate +// || ( +// // Check if the vision estimate is within the distance threshold of the +// // current pose estimate. +// visionEstimatedPose2d.getTranslation().getDistance( +// BaseRobotState.robotPose.getTranslation() +// ) < visionEstimateDistanceThresholdMeters +// // Check if the vision estimate is within the angle threshold of +// // the current pose estimate. Get the absolute value of the +// // difference between the angles constrained from -pi radians to pi +// // radians to find the positive shortest difference. +// && Math.abs( +// MathUtil.angleModulus( +// visionEstimatedPose2d.getRotation() +// .minus(BaseRobotState.robotPose.getRotation()) +// .getRadians() +// ) +// ) < visionEstimateAngleThresholdRadians +// ) ) { Matrix standardDeviations = calculateEstimateStandardDeviations(estimatedRobotPose); // If the standard deviations are the noTrustStdDevs, we'll just throw the diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index eda9f7a0..18b5d5cd 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 From 7ad80d8fa6c0aad35a63008a2ad55bb0754c7df3 Mon Sep 17 00:00:00 2001 From: henrymilbert <110063972+henrymilbert@users.noreply.github.com> Date: Thu, 2 Apr 2026 22:36:10 -0500 Subject: [PATCH 2/3] add pose loss handling based on consecutive discarded estimates --- .../com/team1816/lib/subsystems/Vision.java | 69 ++++++++++++------- 1 file changed, 45 insertions(+), 24 deletions(-) diff --git a/src/main/java/com/team1816/lib/subsystems/Vision.java b/src/main/java/com/team1816/lib/subsystems/Vision.java index fe09e901..23c62f98 100644 --- a/src/main/java/com/team1816/lib/subsystems/Vision.java +++ b/src/main/java/com/team1816/lib/subsystems/Vision.java @@ -59,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. @@ -104,32 +109,34 @@ 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 ( - true -// // 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 -// // vision estimate to the list no matter where it is. -// !BaseRobotState.hasAccuratePoseEstimate -// || ( -// // Check if the vision estimate is within the distance threshold of the -// // current pose estimate. -// visionEstimatedPose2d.getTranslation().getDistance( -// BaseRobotState.robotPose.getTranslation() -// ) < visionEstimateDistanceThresholdMeters -// // Check if the vision estimate is within the angle threshold of -// // the current pose estimate. Get the absolute value of the -// // difference between the angles constrained from -pi radians to pi -// // radians to find the positive shortest difference. -// && Math.abs( -// MathUtil.angleModulus( -// visionEstimatedPose2d.getRotation() -// .minus(BaseRobotState.robotPose.getRotation()) -// .getRadians() -// ) -// ) < visionEstimateAngleThresholdRadians -// ) + // 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 + // vision estimate to the list no matter where it is. + !BaseRobotState.hasAccuratePoseEstimate + || ( + // Check if the vision estimate is within the distance threshold of the + // current pose estimate. + visionEstimatedPose2d.getTranslation().getDistance( + BaseRobotState.robotPose.getTranslation() + ) < visionEstimateDistanceThresholdMeters + // Check if the vision estimate is within the angle threshold of + // the current pose estimate. Get the absolute value of the + // difference between the angles constrained from -pi radians to pi + // radians to find the positive shortest difference. + && Math.abs( + MathUtil.angleModulus( + visionEstimatedPose2d.getRotation() + .minus(BaseRobotState.robotPose.getRotation()) + .getRadians() + ) + ) < 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. @@ -140,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; + } + } } } From ecf46bbe39ab0647ea8694bf1c04f15e5409c70d Mon Sep 17 00:00:00 2001 From: henrymilbert <110063972+henrymilbert@users.noreply.github.com> Date: Fri, 3 Apr 2026 17:57:56 -0500 Subject: [PATCH 3/3] pitch up cameras eight degrees --- src/main/resources/yaml/ztldr.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index 18b5d5cd..e3088ee9 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/src/main/resources/yaml/ztldr.yml @@ -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: