From 01cca27039e8edc4545ba8636f7f0f74cd677cfe Mon Sep 17 00:00:00 2001 From: Saqib Rokadia Date: Fri, 10 Apr 2026 15:07:54 -0700 Subject: [PATCH 1/3] Adding staleness for use with rumble pad. --- .../general_commands/GamepadRumbleCommand.java | 2 +- .../vision/AprilTagVisionSubsystemExtended.java | 13 +++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/java/competition/general_commands/GamepadRumbleCommand.java b/src/main/java/competition/general_commands/GamepadRumbleCommand.java index 3c15d70f..d3f64a85 100644 --- a/src/main/java/competition/general_commands/GamepadRumbleCommand.java +++ b/src/main/java/competition/general_commands/GamepadRumbleCommand.java @@ -31,7 +31,7 @@ public GamepadRumbleCommand(HoodSubsystem hoodSubsystem, OperatorInterface oi, S @Override public void execute() { - var lookAtPointFault = drive.getLookAtPointActive() && !vision.areAllCamerasConnected(); + var lookAtPointFault = drive.getLookAtPointActive() && !vision.hasRecentPoseObservation(); var shooterReady = DriverStation.isTeleop() && shooter.isReadyToFire() && hood.isMaintainerAtGoal(); if (shooterReady) { diff --git a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java index 506665ac..45515a73 100644 --- a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java +++ b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.Timer; import xbot.common.injection.electrical_contract.CameraInfo; import xbot.common.injection.electrical_contract.XCameraElectricalContract; import xbot.common.properties.PropertyFactory; @@ -24,6 +25,7 @@ public class AprilTagVisionSubsystemExtended extends AprilTagVisionSubsystem { HashMap aprilTagIDHashMap = new HashMap<>(); private final AprilTagFieldLayout aprilTagFieldLayout; public final CameraInfo[] cameras; + private static final double STALE_THRESHOLD_SEC = 0.200; @Inject public AprilTagVisionSubsystemExtended(PropertyFactory pf, @@ -114,4 +116,15 @@ public boolean areAllCamerasConnected() { } return true; } + + public boolean hasRecentPoseObservation() { + for (var poseObservation : this.getAllPoseObservations()) { + boolean stale = Timer.getFPGATimestamp() - poseObservation.getTimestampSeconds() > STALE_THRESHOLD_SEC; + if (!stale) { + return false; + } + } + + return true; + } } From b8e1716e3c60bbb992357f7799d0ae9bc24e9394 Mon Sep 17 00:00:00 2001 From: Saqib Rokadia Date: Fri, 10 Apr 2026 17:36:31 -0700 Subject: [PATCH 2/3] Making into a property. --- .../subsystems/vision/AprilTagVisionSubsystemExtended.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java index 45515a73..d5c5dfd2 100644 --- a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java +++ b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.Timer; import xbot.common.injection.electrical_contract.CameraInfo; import xbot.common.injection.electrical_contract.XCameraElectricalContract; +import xbot.common.properties.DoubleProperty; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.vision.AprilTagVisionIO; import xbot.common.subsystems.vision.AprilTagVisionIOFactory; @@ -25,7 +26,7 @@ public class AprilTagVisionSubsystemExtended extends AprilTagVisionSubsystem { HashMap aprilTagIDHashMap = new HashMap<>(); private final AprilTagFieldLayout aprilTagFieldLayout; public final CameraInfo[] cameras; - private static final double STALE_THRESHOLD_SEC = 0.200; + private final DoubleProperty stalenessThresholdInSeconds; @Inject public AprilTagVisionSubsystemExtended(PropertyFactory pf, @@ -50,6 +51,8 @@ public AprilTagVisionSubsystemExtended(PropertyFactory pf, // aprilTagIDHashMap.put(Landmarks.BlueFarAlgae, 21); // aprilTagIDHashMap.put(Landmarks.BlueFarRightAlgae, 22); + pf.setPrefix("AprilTagVisionSubsystemExtended"); + this.stalenessThresholdInSeconds = pf.createPersistentProperty("Staleness Threshold In Seconds", 0.2); aprilTagFieldLayout = fieldLayout; } @@ -119,7 +122,7 @@ public boolean areAllCamerasConnected() { public boolean hasRecentPoseObservation() { for (var poseObservation : this.getAllPoseObservations()) { - boolean stale = Timer.getFPGATimestamp() - poseObservation.getTimestampSeconds() > STALE_THRESHOLD_SEC; + boolean stale = Timer.getFPGATimestamp() - poseObservation.timestampSeconds() > this.stalenessThresholdInSeconds.get(); if (!stale) { return false; } From 4e431c09ca53c851f25eaee6b7b18cf4466c8e96 Mon Sep 17 00:00:00 2001 From: Saqib Rokadia Date: Sat, 11 Apr 2026 09:49:12 -0700 Subject: [PATCH 3/3] Fixing logic flaw. --- .../subsystems/vision/AprilTagVisionSubsystemExtended.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java index d5c5dfd2..ec85a157 100644 --- a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java +++ b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java @@ -124,10 +124,10 @@ public boolean hasRecentPoseObservation() { for (var poseObservation : this.getAllPoseObservations()) { boolean stale = Timer.getFPGATimestamp() - poseObservation.timestampSeconds() > this.stalenessThresholdInSeconds.get(); if (!stale) { - return false; + return true; } } - return true; + return false; } }