From d2b5df3a8a9d97aa8fbcb494636f3d1326e70798 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 10 Mar 2026 08:55:51 -0400 Subject: [PATCH 1/6] (#108) Add tilt and freefall detection for ignoring bump odometry --- .../deploy/constants/comp/DriveConstants.json | 12 +++++++ .../test_drivebase/DriveConstants.json | 12 +++++++ .../robot/constants/drive/DriveConstants.java | 21 +++++++++++++ .../frc/robot/subsystems/drive/Drive.java | 31 +++++++++++++++++++ .../frc/robot/subsystems/drive/GyroIO.java | 4 +++ .../robot/subsystems/drive/GyroIOPigeon2.java | 15 ++++++++- 6 files changed, 94 insertions(+), 1 deletion(-) 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..87a94abf 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 vision. 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..fb0dff60 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); } /** @@ -396,6 +421,12 @@ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { + if (bumpDetected) { + visionMeasurementStdDevs.set(0, 0, 0.0); + visionMeasurementStdDevs.set(1, 0, 0.0); + visionMeasurementStdDevs.set(2, 0, 0.0); + } + if (JsonConstants.featureFlags.useMAPoseEstimator) { maPoseEstimator.addVisionData( List.of( 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..6d0f3807 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) { @@ -65,5 +74,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); } } From 9721de317f65a72b3468a92a108a1d5dacb2e3cb Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 10 Mar 2026 09:04:26 -0400 Subject: [PATCH 2/6] (#108) Fix update frequencies, bad javadoc, and stop mutating std deviations of vision estimate --- .../frc/robot/constants/drive/DriveConstants.java | 4 ++-- .../java/frc/robot/subsystems/drive/Drive.java | 15 +++++---------- .../frc/robot/subsystems/drive/GyroIOPigeon2.java | 4 ++++ 3 files changed, 11 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/constants/drive/DriveConstants.java b/src/main/java/frc/robot/constants/drive/DriveConstants.java index 87a94abf..a8a42485 100644 --- a/src/main/java/frc/robot/constants/drive/DriveConstants.java +++ b/src/main/java/frc/robot/constants/drive/DriveConstants.java @@ -60,8 +60,8 @@ public class DriveConstants { 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 vision. This - * value should be as small as possible without accidentally trusting odometry while still + * 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); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index fb0dff60..51959afe 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -416,25 +416,20 @@ public void setPose(Pose2d pose) { } } + private final Matrix zeroStdDevs = VecBuilder.fill(0.0, 0.0, 0.0); + /** Adds a new timestamped vision measurement. */ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - if (bumpDetected) { - visionMeasurementStdDevs.set(0, 0, 0.0); - visionMeasurementStdDevs.set(1, 0, 0.0); - visionMeasurementStdDevs.set(2, 0, 0.0); - } + var stdDevs = bumpDetected ? zeroStdDevs : 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/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 6d0f3807..42bdb1ed 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -57,6 +57,10 @@ public GyroIOPigeon2() { yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone()); + pitch.setUpdateFrequency(50.0); + roll.setUpdateFrequency(50.0); + zAccel.setUpdateFrequency(50.0); + StatusSignalRefresher.addSignals(JsonConstants.robotInfo.CANBus, signals); } From eaa32f152e811a8d3b4470a4a7f0bee983738fec Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 10 Mar 2026 09:16:24 -0400 Subject: [PATCH 3/6] (#108) Set update frequences before optimizing bus utilization for gyroIO pigeon2 --- .../java/frc/robot/subsystems/drive/GyroIOPigeon2.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 42bdb1ed..f12fd9d8 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -53,14 +53,16 @@ public GyroIOPigeon2() { pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(JsonConstants.robotInfo.ODOMETRY_FREQUENCY); yawVelocity.setUpdateFrequency(50.0); - pigeon.optimizeBusUtilization(); - yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone()); pitch.setUpdateFrequency(50.0); roll.setUpdateFrequency(50.0); zAccel.setUpdateFrequency(50.0); + pigeon.optimizeBusUtilization(); + + yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone()); + StatusSignalRefresher.addSignals(JsonConstants.robotInfo.CANBus, signals); } From 464feb28134337bd0fbb453b2f8fa4801971310f Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 10 Mar 2026 10:49:24 -0400 Subject: [PATCH 4/6] (#108) Use a tiny distance instead of 0.0 for bump vision std devs --- .../java/frc/robot/subsystems/drive/Drive.java | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 51959afe..acd49c29 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -416,14 +416,26 @@ public void setPose(Pose2d pose) { } } - private final Matrix zeroStdDevs = VecBuilder.fill(0.0, 0.0, 0.0); + // Assume we're at a certain tiny distance away from every tag when dropping standard deviations + 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 ? zeroStdDevs : visionMeasurementStdDevs; + var stdDevs = bumpDetected ? droppedStdDevs : visionMeasurementStdDevs; if (JsonConstants.featureFlags.useMAPoseEstimator) { maPoseEstimator.addVisionData( From 8e4572337d0fc08d1ee6e63c154d0a4fef5fa2fe Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 10 Mar 2026 15:36:17 -0400 Subject: [PATCH 5/6] (#108) Document pitch, roll, and z acceleration update frequency changes --- src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index f12fd9d8..c9cbde05 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -54,6 +54,10 @@ public GyroIOPigeon2() { 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); From a1a0c681c12a9d875477f152dce8b92fffba957d Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 10 Mar 2026 15:41:48 -0400 Subject: [PATCH 6/6] (#108) Better document standard deviation approximation --- src/main/java/frc/robot/subsystems/drive/Drive.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index acd49c29..359b53bb 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -417,6 +417,13 @@ 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(