From ea6b204c5082e825147865597b0f0887740d77ed Mon Sep 17 00:00:00 2001 From: tori lee <144298063+tortillini-tokki@users.noreply.github.com> Date: Sun, 5 Apr 2026 23:21:28 -0700 Subject: [PATCH 1/2] add bump detection and camera cleaning --- src/main/java/frc/robot/Constants.java | 3 +++ src/main/java/frc/robot/RobotContainer.java | 15 +++++++++++++++ .../drivetrain/DrivetrainCommandFactory.java | 8 ++++++++ .../subsystems/vision/LocalizationCamera.java | 10 ++++++++++ .../robot/subsystems/vision/VisionSubsystem.java | 8 ++++++++ 5 files changed, 44 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6a11e5f1..13ac0781 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -152,6 +152,9 @@ public static class DrivetrainConstants { // velocity tolerance (in m/s) for velocityNonZero() public static final double VELOCITY_TOLERANCE = 0.2; + // Bump detection threshold (in g's from Pigeon2 accelerometer) + public static final double BUMP_ACCELERATION_THRESHOLD = 1.5; + } public static class PivotConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc9cbe12..3472999b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,6 +43,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -340,6 +341,20 @@ private void configureBindingsVision() { // --- CONFIGURE RAW VIDEO MODE TOGGLE --- m_testJoystick.start().onTrue(m_vision.toggleRawVideoModeCommand()); + + // --- CONFIGURE BUMP DETECTION AND CAMERA HISTORY CLEANING --- + // automatic -- todo: reality check / tune debounce? + m_drivetrainCommandFactory.bumpDetectedTrigger().debounce(0.5).onTrue( + Commands.runOnce(() -> { + m_vision.clearAllCamerasBeforeTimestamp(m_drivetrain.getState().Timestamp); + }) + ); + // manual clear history - todo: pick something for manual clearing? + m_operatorJoystick.back().onTrue( + Commands.runOnce(() -> { + m_vision.clearAllCamerasBeforeTimestamp(m_drivetrain.getState().Timestamp); + }) + ); } private void configureBindingsSimulation() { diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java index 9cee0b8f..d033f01b 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java @@ -198,6 +198,14 @@ public Trigger atAngleTrigger(Supplier angleSupplier) { return new Trigger(() -> atAngle(angleSupplier)); } + // ----- BUMP DETECTION ----- + public Trigger bumpDetectedTrigger() { + return new Trigger(() -> { + double accelZ = Math.abs(m_drivetrain.getPigeon2().getAccelerationZ().getValueAsDouble()); + return accelZ > DrivetrainConstants.BUMP_ACCELERATION_THRESHOLD; + }); + } + // ----- SYSID ----- public Command sysIdQuasistaticTranslationForward() { return m_drivetrain.sysIdQuasistaticTranslation(Direction.kForward); diff --git a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java index 40de884a..0b172837 100644 --- a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java +++ b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java @@ -110,6 +110,16 @@ public void setRawVideoMode(boolean rawVideoMode) { SmartDashboard.putBoolean(m_logString + "/rawVideoModeEnabled", rawVideoMode); } + // method that clears all camera readings from before a certain timestamp by removing them from m_lastReadings + public void clearReadingsBeforeTimestamp(double timestamp) { + m_lastReadings.removeIf(reading -> reading.timestampSeconds() < timestamp); + + // also clear current reading if it's stale / pre-timestamp + if (m_currentReading.isPresent() && m_currentReading.get().timestampSeconds() < timestamp) { + m_currentReading = Optional.empty(); + } + } + // --- filtering methods --- public void setFilterPipeline(LocalVisionFilterPipeline filterPipeline) { m_filterPipeline = filterPipeline; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 404c6440..a231ee99 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -111,6 +111,14 @@ public Command toggleRawVideoModeCommand() { }); } + // loops through all cameras and clears all readings before a certain timestamp + // used for bump detection / cleaning + public void clearAllCamerasBeforeTimestamp(double timestamp) { + for (LocalizationCamera cam : cameras) { + cam.clearReadingsBeforeTimestamp(timestamp); + } + } + @Override public void periodic() { // clear allValidReadings at the beginning of every loop From 172b9fc03d3758be7b4a59ffd5a5642ca92f51ab Mon Sep 17 00:00:00 2001 From: tori lee <144298063+tortillini-tokki@users.noreply.github.com> Date: Mon, 6 Apr 2026 08:42:41 -0700 Subject: [PATCH 2/2] change debounce to constant --- src/main/java/frc/robot/Constants.java | 3 +++ src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 13ac0781..b0e6cafa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -154,6 +154,9 @@ public static class DrivetrainConstants { // Bump detection threshold (in g's from Pigeon2 accelerometer) public static final double BUMP_ACCELERATION_THRESHOLD = 1.5; + + // Bump detection debounce, in seconds + public static final double BUMP_DETECTION_DEBOUNCE = 0.5; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3472999b..6e570d19 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -344,7 +344,7 @@ private void configureBindingsVision() { // --- CONFIGURE BUMP DETECTION AND CAMERA HISTORY CLEANING --- // automatic -- todo: reality check / tune debounce? - m_drivetrainCommandFactory.bumpDetectedTrigger().debounce(0.5).onTrue( + m_drivetrainCommandFactory.bumpDetectedTrigger().debounce(DrivetrainConstants.BUMP_DETECTION_DEBOUNCE).onTrue( Commands.runOnce(() -> { m_vision.clearAllCamerasBeforeTimestamp(m_drivetrain.getState().Timestamp); })