diff --git a/src/main/deploy/constants/comp/DriveConstants.json b/src/main/deploy/constants/comp/DriveConstants.json index ba534635..87d0012f 100644 --- a/src/main/deploy/constants/comp/DriveConstants.json +++ b/src/main/deploy/constants/comp/DriveConstants.json @@ -58,5 +58,17 @@ "kG": 0.0, "kV": 0.82219, "kA": 0.0 + }, + "bumpOdometryIgnoreTime": { + "value": 0.25, + "unit": "Second" + }, + "maxOdometryTilt": { + "value": 5.0, + "unit": "Degree" + }, + "maxOdometryZAcceleration": { + "value": 5.0, + "unit": "Meter per Second per Second" } } diff --git a/src/main/deploy/constants/test_drivebase/DriveConstants.json b/src/main/deploy/constants/test_drivebase/DriveConstants.json index b4598338..f3ed3fbd 100644 --- a/src/main/deploy/constants/test_drivebase/DriveConstants.json +++ b/src/main/deploy/constants/test_drivebase/DriveConstants.json @@ -58,5 +58,17 @@ "kG": 0.0, "kV": 0.75722, "kA": 0.0 + }, + "bumpOdometryIgnoreTime": { + "value": 0.25, + "unit": "Second" + }, + "maxOdometryTilt": { + "value": 5.0, + "unit": "Degree" + }, + "maxOdometryZAcceleration": { + "value": 5.0, + "unit": "Meter per Second per Second" } } diff --git a/src/main/java/frc/robot/constants/drive/DriveConstants.java b/src/main/java/frc/robot/constants/drive/DriveConstants.java index 6dc1b4db..a8a42485 100644 --- a/src/main/java/frc/robot/constants/drive/DriveConstants.java +++ b/src/main/java/frc/robot/constants/drive/DriveConstants.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; @@ -13,6 +14,7 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Time; import frc.robot.util.PIDGains; /** @@ -56,4 +58,23 @@ public class DriveConstants { public PIDGains steerGains = new PIDGains(100, 0.0, 0.5, 0.1, 0.0, 2.49, 0.0); public PIDGains driveGains = new PIDGains(0.1, 0.0, 0.0, 0.20845, 0.0, 0.75722, 0.0); + + /** + * How much time after we last detected that we were on the bump to continue ignoring odometry. + * This value should be as small as possible without accidentally trusting odometry while still + * on/falling off of the bump + */ + public Time bumpOdometryIgnoreTime = Seconds.of(0.25); + + /** + * The maximum cumulative tilt (abs(pitch) + abs(roll)) that may be present before the bump + * odometry ignorance is triggered. + */ + public Angle maxOdometryTilt = Degrees.of(5.0); + + /** + * The maximum z acceleration (abs(z acceleration)) that may be present before the bump odometry + * ignorance is triggered. + */ + public LinearAcceleration maxOdometryZAcceleration = MetersPerSecondPerSecond.of(5.0); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index fbca84ec..359b53bb 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -18,6 +18,8 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -94,6 +96,17 @@ public class Drive extends SubsystemBase implements DriveTemplate { private PoseEstimator maPoseEstimator = new PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.0002)); + /** + * Whether or not we think we're on or just came off of the bump and should fully trust vision to + * prevent crazy odometry. + */ + @AutoLogOutput(key = "Odometry/BumpDetected") + private boolean bumpDetected = false; + + private final Debouncer bumpOdometryDebouncer = + new Debouncer( + JsonConstants.driveConstants.bumpOdometryIgnoreTime.in(Seconds), DebounceType.kFalling); + public Drive( GyroIO gyroIO, ModuleIO flModuleIO, @@ -232,6 +245,18 @@ public void periodic() { // Update gyro alert gyroDisconnectedAlert.set(!gyroInputs.connected && Constants.currentMode != Mode.SIM); + + // Check for bump + double cumulativeTiltRadians = + Math.abs(gyroInputs.rollRadians) + Math.abs(gyroInputs.pitchRadians); + double zAccelMagnitude = Math.abs(gyroInputs.zAccelerationMetersPerSecondPerSecond); + boolean bumpDetectedNow = + cumulativeTiltRadians > JsonConstants.driveConstants.maxOdometryTilt.in(Radians) + || zAccelMagnitude + > JsonConstants.driveConstants.maxOdometryZAcceleration.in( + MetersPerSecondPerSecond); + + this.bumpDetected = bumpOdometryDebouncer.calculate(bumpDetectedNow); } /** @@ -391,19 +416,39 @@ public void setPose(Pose2d pose) { } } + // Assume we're at a certain tiny distance away from every tag when dropping standard deviations + // This is used in place of just setting all standard deviations to zero in case pose estimators + // have genuine mathematical issues dealing with zero. Vision should be trusted very very heavily + // when odometry is untrustworthy to avoid allowing the pose estimate to get super wrong. + // This value is squared and then multiplied by the linear/angular std dev factors to match the + // formulas for std dev calculations in coppercore vision. + // Think of this like setting the trust level while odometry is untrustworthy to be equivalent to + // seeing 1 tag at this distance away from the camera. This distance is in meters. + private final double droppedStdDevApproxDistance = 0.01; + private final Matrix droppedStdDevs = + VecBuilder.fill( + JsonConstants.visionConstants.gainConstants.linearStdDevFactor + * droppedStdDevApproxDistance + * droppedStdDevApproxDistance, + JsonConstants.visionConstants.gainConstants.linearStdDevFactor + * droppedStdDevApproxDistance + * droppedStdDevApproxDistance, + JsonConstants.visionConstants.gainConstants.angularStdDevFactor + * droppedStdDevApproxDistance + * droppedStdDevApproxDistance); + /** Adds a new timestamped vision measurement. */ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { + var stdDevs = bumpDetected ? droppedStdDevs : visionMeasurementStdDevs; + if (JsonConstants.featureFlags.useMAPoseEstimator) { maPoseEstimator.addVisionData( - List.of( - new TimestampedVisionUpdate( - timestampSeconds, visionRobotPoseMeters, visionMeasurementStdDevs))); + List.of(new TimestampedVisionUpdate(timestampSeconds, visionRobotPoseMeters, stdDevs))); } else { - poseEstimator.addVisionMeasurement( - visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); + poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds, stdDevs); } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 4e9754f9..57942805 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -18,6 +18,10 @@ public static class GyroIOInputs { public double yawVelocityRadPerSec = 0.0; public double[] odometryYawTimestamps = new double[] {}; public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + + public double pitchRadians = 0.0; + public double rollRadians = 0.0; + public double zAccelerationMetersPerSecondPerSecond = 0.0; } public default void updateInputs(GyroIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 8a20b908..c9cbde05 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -7,6 +7,9 @@ package frc.robot.subsystems.drive; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Radians; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; @@ -16,6 +19,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearAcceleration; import frc.robot.constants.JsonConstants; import java.util.Queue; @@ -30,7 +34,12 @@ public class GyroIOPigeon2 implements GyroIO { private final Queue yawTimestampQueue; private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - private final BaseStatusSignal[] signals = new BaseStatusSignal[] {yaw, yawVelocity}; + private final StatusSignal pitch = pigeon.getPitch(); + private final StatusSignal roll = pigeon.getRoll(); + private final StatusSignal zAccel = pigeon.getAccelerationZ(); + + private final BaseStatusSignal[] signals = + new BaseStatusSignal[] {yaw, yawVelocity, pitch, roll, zAccel}; public GyroIOPigeon2() { if (JsonConstants.physicalDriveConstants.DrivetrainConstants.Pigeon2Configs != null) { @@ -44,7 +53,17 @@ public GyroIOPigeon2() { pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(JsonConstants.robotInfo.ODOMETRY_FREQUENCY); yawVelocity.setUpdateFrequency(50.0); + + // These signals are refreshed at the periodic rate (50hz) (just like yaw velocity) rather than + // the odometry frequency (like yaw), because they are used for robot code decisionmaking rather + // than high frequency odometry. + // This shouldn't have any effect on the currently existing odometry code + pitch.setUpdateFrequency(50.0); + roll.setUpdateFrequency(50.0); + zAccel.setUpdateFrequency(50.0); + pigeon.optimizeBusUtilization(); + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone()); @@ -65,5 +84,9 @@ public void updateInputs(GyroIOInputs inputs) { .toArray(Rotation2d[]::new); yawTimestampQueue.clear(); yawPositionQueue.clear(); + + inputs.pitchRadians = pitch.getValue().in(Radians); + inputs.rollRadians = roll.getValue().in(Radians); + inputs.zAccelerationMetersPerSecondPerSecond = zAccel.getValue().in(MetersPerSecondPerSecond); } }