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..ec85a157 100644 --- a/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java +++ b/src/main/java/competition/subsystems/vision/AprilTagVisionSubsystemExtended.java @@ -7,8 +7,10 @@ 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.DoubleProperty; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.vision.AprilTagVisionIO; import xbot.common.subsystems.vision.AprilTagVisionIOFactory; @@ -24,6 +26,7 @@ public class AprilTagVisionSubsystemExtended extends AprilTagVisionSubsystem { HashMap aprilTagIDHashMap = new HashMap<>(); private final AprilTagFieldLayout aprilTagFieldLayout; public final CameraInfo[] cameras; + private final DoubleProperty stalenessThresholdInSeconds; @Inject public AprilTagVisionSubsystemExtended(PropertyFactory pf, @@ -48,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; } @@ -114,4 +119,15 @@ public boolean areAllCamerasConnected() { } return true; } + + public boolean hasRecentPoseObservation() { + for (var poseObservation : this.getAllPoseObservations()) { + boolean stale = Timer.getFPGATimestamp() - poseObservation.timestampSeconds() > this.stalenessThresholdInSeconds.get(); + if (!stale) { + return true; + } + } + + return false; + } }