diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6a11e5f1..b0e6cafa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -152,6 +152,12 @@ 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; + + // Bump detection debounce, in seconds + public static final double BUMP_DETECTION_DEBOUNCE = 0.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..6e570d19 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(DrivetrainConstants.BUMP_DETECTION_DEBOUNCE).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