diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1ca9709d..c8b6ca22 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; @@ -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 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 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(); 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; }); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 18207aee..f7b9c739 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/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 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