add bump detection and camera cleaning

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 {

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() {

src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java

@@ -198,6 +198,14 @@ public Trigger atAngleTrigger(Supplier<Rotation2d> 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);

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;

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