From 632a2dd981a3f9d353863261b2aaeeb97350d48d Mon Sep 17 00:00:00 2001 From: tori lee <144298063+tortillini-tokki@users.noreply.github.com> Date: Mon, 6 Apr 2026 09:12:37 -0700 Subject: [PATCH 1/4] reset drivetrain to average of first new vision readings! --- src/main/java/frc/robot/RobotContainer.java | 24 +++++++++++++++++++ .../subsystems/vision/VisionSubsystem.java | 19 +++++++++++++++ 2 files changed, 43 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6e570d19..4d71b242 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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; @@ -136,6 +138,10 @@ public boolean isMagnitudeGreaterThan(double threshold) { private boolean scoreMode = false; + // 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; + /** The container for the robot. Contains subsystems and commands. */ public RobotContainer() { @@ -347,8 +353,12 @@ private void configureBindingsVision() { m_drivetrainCommandFactory.bumpDetectedTrigger().debounce(DrivetrainConstants.BUMP_DETECTION_DEBOUNCE).onTrue( Commands.runOnce(() -> { m_vision.clearAllCamerasBeforeTimestamp(m_drivetrain.getState().Timestamp); + + // 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(() -> { @@ -613,6 +623,20 @@ public Command getAutonomousCommand() { public void updatePoseEst() { List 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 && !allReadings.isEmpty()) { + Translation2d avgTranslation = m_vision.getAverageTranslation(allReadings); + + 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; + return; // Skip normal fusion this cycle + } + } + for (CameraReading reading : allReadings){ EstimatedRobotPose robotPose = reading.robotPose(); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index a231ee99..c71de175 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -119,6 +119,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 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 From 3fea1be96305e65d0bb9f656f095a40cdbb5ecc8 Mon Sep 17 00:00:00 2001 From: zoe Date: Mon, 6 Apr 2026 17:24:41 -0700 Subject: [PATCH 2/4] log values and fix calculation --- .../subsystems/drivetrain/DrivetrainCommandFactory.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java index d033f01b..f72beb04 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainCommandFactory.java @@ -201,7 +201,10 @@ public Trigger atAngleTrigger(Supplier 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; }); } From 68486f3dd7307e6938ac520d0d1cc68b2c8bdaa7 Mon Sep 17 00:00:00 2001 From: zoe Date: Mon, 6 Apr 2026 17:56:56 -0700 Subject: [PATCH 3/4] init m_resetPoseOnNextVisionReading to false --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 057f3123..9161131c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -140,7 +140,7 @@ public boolean isMagnitudeGreaterThan(double threshold) { // 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; + private boolean m_resetPoseOnNextVisionReading = false; /** The container for the robot. Contains subsystems and commands. */ public RobotContainer() { From d7461757135f93465e85f3fba26c230f804ca884 Mon Sep 17 00:00:00 2001 From: zoe Date: Mon, 6 Apr 2026 18:06:40 -0700 Subject: [PATCH 4/4] check resetPose reading timestamp --- src/main/java/frc/robot/RobotContainer.java | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c9d786a8..c8b6ca22 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -141,6 +141,7 @@ public boolean isMagnitudeGreaterThan(double threshold) { // 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() { @@ -354,7 +355,9 @@ 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; @@ -638,14 +641,23 @@ public void updatePoseEst() { // If we just crossed the bump and have readings, hard reset drivetrain odometry // NOTE: want to SKIP normal fusion. - if (m_resetPoseOnNextVisionReading && !allReadings.isEmpty()) { - Translation2d avgTranslation = m_vision.getAverageTranslation(allReadings); + if (m_resetPoseOnNextVisionReading) { + List 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 } }