reset drivetrain to average of first new vision readings

src/main/java/frc/robot/RobotContainer.java

@@ -58,8 +58,10 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 
+import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
 import java.util.function.Consumer;
@@ -115,7 +117,6 @@ public boolean isMagnitudeGreaterThan(double threshold) {
     SensorConstants.FEEDER_SENSOR_MIN_DISTANCE
   );
 
-  // Score mode state
   private boolean scoreMode = false;
 
   // Command factories
@@ -137,6 +138,11 @@ public boolean isMagnitudeGreaterThan(double threshold) {
 
   private final Field2d m_actualField = new Field2d(); // field simulation
 
+  // boolean to keep track of when we want to reset drivetrain to vision reading
+  // used for drivetrain "fixing" after bump detection
+  private boolean m_resetPoseOnNextVisionReading = false;
+  private double m_resetPoseMinTimestamp = Double.NEGATIVE_INFINITY;
+
   /** The container for the robot. Contains subsystems and commands. */
   public RobotContainer() {
 
@@ -349,9 +355,15 @@ private void configureBindingsVision() {
     // automatic -- todo: reality check / tune debounce?
     m_drivetrainCommandFactory.bumpDetectedTrigger().debounce(DrivetrainConstants.BUMP_DETECTION_DEBOUNCE).onTrue(
       Commands.runOnce(() -> {
-        m_vision.clearAllCamerasBeforeTimestamp(m_drivetrain.getState().Timestamp);
+        double clearTimestamp = m_drivetrain.getState().Timestamp;
+        m_vision.clearAllCamerasBeforeTimestamp(clearTimestamp);
+        m_resetPoseMinTimestamp = clearTimestamp;
+
+        // after crossing the bump, we want to reset drivetrain pose to first vision pose
+        m_resetPoseOnNextVisionReading = true;
       })
     );
+
     // manual clear history - todo: pick something for manual clearing?
     m_operatorJoystick.back().onTrue(
       Commands.runOnce(() -> {
@@ -627,6 +639,29 @@ public Command getAutonomousCommand() {
   public void updatePoseEst() {
     List<CameraReading> allReadings = m_vision.getValidCameraReadings();
 
+    // If we just crossed the bump and have readings, hard reset drivetrain odometry
+    // NOTE: want to SKIP normal fusion.
+    if (m_resetPoseOnNextVisionReading) {
+      List<CameraReading> postBumpReadings = allReadings.stream()
+        .filter(reading -> reading.timestampSeconds() > m_resetPoseMinTimestamp)
+        .toList();
+
+      if (postBumpReadings.isEmpty()) {
+        return;
+      }
+
+      Translation2d avgTranslation = m_vision.getAverageTranslation(postBumpReadings);
+
+      if (avgTranslation != null) {
+        // Create Pose2d from averaged translation, keeping gyro heading (still pretty accurate after bump)
+        Pose2d visionPose = new Pose2d(avgTranslation, m_drivetrain.getState().Pose.getRotation());
+        m_drivetrain.resetPose(visionPose);
+        m_resetPoseOnNextVisionReading = false;
+        m_resetPoseMinTimestamp = Double.NEGATIVE_INFINITY;
+        return; // Skip normal fusion this cycle
+      }
+    }
+
     for (CameraReading reading : allReadings){
       EstimatedRobotPose robotPose = reading.robotPose();
 

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

@@ -201,7 +201,10 @@ public Trigger atAngleTrigger(Supplier<Rotation2d> angleSupplier) {
     // ----- BUMP DETECTION -----
     public Trigger bumpDetectedTrigger() {
         return new Trigger(() -> {
-            double accelZ = Math.abs(m_drivetrain.getPigeon2().getAccelerationZ().getValueAsDouble());
+            double accelZ = Math.abs(m_drivetrain.getPigeon2().getAccelerationZ().getValueAsDouble() - 1); // compensate for gravity 
+            SmartDashboard.putBoolean("drivetrain/bumpDetected", accelZ > DrivetrainConstants.BUMP_ACCELERATION_THRESHOLD);
+            SmartDashboard.putNumber("drivetrain/pigeonAccelZGravityCompensatedGs", m_drivetrain.getPigeon2().getAccelerationZ().getValueAsDouble() - 1);
+
             return accelZ > DrivetrainConstants.BUMP_ACCELERATION_THRESHOLD;
         });
     }

src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java

@@ -125,6 +125,25 @@ public void clearAllCamerasBeforeTimestamp(double timestamp) {
     }
   }
 
+  // averages all camera readings given input list of camera readings
+  // @param allReadings is list of camera readings to average
+  public Translation2d getAverageTranslation(List<CameraReading> allReadings) {
+    if (allReadings.isEmpty()) {
+      return null;
+    }
+
+    double totalX = 0.0;
+    double totalY = 0.0;
+    for (CameraReading reading : allReadings) {
+      totalX += reading.robotPose().estimatedPose.getX();
+      totalY += reading.robotPose().estimatedPose.getY();
+    }
+
+    int totalReadings = allReadings.size();
+
+    return new Translation2d(totalX / totalReadings, totalY / totalReadings);
+  }
+
   @Override
   public void periodic() {
     // clear allValidReadings at the beginning of every loop