From 62a864d2f6b0caf9a07a1dc3d5e3da160957a718 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 2 Feb 2026 18:14:19 -0700 Subject: [PATCH 1/6] Clean up the Accelerometer / IMU subsystems Clean up the code, but also specifically allow the RIO or IMU to be "on its side" (i.e., Z is not necessarily "up"). --- src/main/java/frc/robot/Constants.java | 17 ++-- .../accelerometer/Accelerometer.java | 82 +++++-------------- .../java/frc/robot/subsystems/imu/Imu.java | 12 +-- .../java/frc/robot/subsystems/imu/ImuIO.java | 11 +-- .../frc/robot/subsystems/imu/ImuIONavX.java | 53 ++++-------- .../robot/subsystems/imu/ImuIOPigeon2.java | 44 +++++----- .../frc/robot/subsystems/imu/ImuIOSim.java | 24 +++--- 7 files changed, 90 insertions(+), 153 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0b39dcc..5b76500 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -126,19 +126,18 @@ public static final class RobotConstants { // Insert here the orientation (CCW == +) of the Rio and IMU from the robot // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference // frame. - // NOTE: It is assumed that both the Rio and the IMU are mounted such that +Z is UP - public static final Rotation2d kRioOrientation = + public static final Rotation3d kRioOrientation = switch (getRobot()) { - case COMPBOT -> Rotation2d.fromDegrees(-90.); - case DEVBOT -> Rotation2d.fromDegrees(0.); - default -> Rotation2d.fromDegrees(0.); + case COMPBOT -> new Rotation3d(0, 0, -90); + case DEVBOT -> Rotation3d.kZero; + default -> Rotation3d.kZero; }; // IMU can be one of Pigeon2 or NavX - public static final Rotation2d kIMUOrientation = + public static final Rotation3d kIMUOrientation = switch (getRobot()) { - case COMPBOT -> Rotation2d.fromDegrees(0.); - case DEVBOT -> Rotation2d.fromDegrees(0.); - default -> Rotation2d.fromDegrees(0.); + case COMPBOT -> Rotation3d.kZero; + case DEVBOT -> Rotation3d.kZero; + default -> Rotation3d.kZero; }; } diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 3bfc5c1..fcb7dd6 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.accelerometer; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; import frc.robot.Constants.RobotConstants; @@ -26,30 +27,20 @@ *

This virtual subsystem pulls the acceleration values from both the RoboRIO and the swerve's * IMU (either Pigeon2 or NavX) and logs them to both AdvantageKitd. In addition to the * accelerations, the jerk (a-dot or x-tripple-dot) is computed from the delta accelerations. - * - *

Primitive-only hot path: no WPILib geometry objects or Units objects. */ public class Accelerometer extends VirtualSubsystem { + + // Gravitational acceleration private static final double G_TO_MPS2 = 9.80665; + // Define hardware interfaces private final RioAccelIO rio; private final RioAccelIO.Inputs rioInputs = new RioAccelIO.Inputs(); private final Imu imu; - // Precompute yaw-only rotation terms - private static final double rioCos = Math.cos(RobotConstants.kRioOrientation.getRadians()); - private static final double rioSin = Math.sin(RobotConstants.kRioOrientation.getRadians()); - private static final double imuCos = Math.cos(RobotConstants.kIMUOrientation.getRadians()); - private static final double imuSin = Math.sin(RobotConstants.kIMUOrientation.getRadians()); - - // Previous Rio accel (m/s^2) - private double prevRioAx = 0.0, prevRioAy = 0.0, prevRioAz = 0.0; - - // Reusable arrays for logging - private final double[] rioAccelArr = new double[3]; - private final double[] rioJerkArr = new double[3]; - private final double[] imuAccelArr = new double[3]; - private final double[] imuJerkArr = new double[3]; + // Variables needed during the periodic + private Translation3d rawRio, rioAcc, rioJerk, imuAcc, imuJerk; + private Translation3d prevRioAcc = Translation3d.kZero; // Log decimation private int loopCount = 0; @@ -74,58 +65,27 @@ public void rbsiPeriodic() { rio.updateInputs(rioInputs); // Compute RIO accelerations and jerks - final double rawX = rioInputs.xG; - final double rawY = rioInputs.yG; - final double rawZ = rioInputs.zG; - - final double rioAx = (rioCos * rawX - rioSin * rawY) * G_TO_MPS2; - final double rioAy = (rioSin * rawX + rioCos * rawY) * G_TO_MPS2; - final double rioAz = rawZ * G_TO_MPS2; - - final double rioJx = (rioAx - prevRioAx) / Constants.loopPeriodSecs; - final double rioJy = (rioAy - prevRioAy) / Constants.loopPeriodSecs; - final double rioJz = (rioAz - prevRioAz) / Constants.loopPeriodSecs; + rawRio = new Translation3d(rioInputs.xG, rioInputs.yG, rioInputs.zG); + rioAcc = rawRio.rotateBy(RobotConstants.kRioOrientation).times(G_TO_MPS2); // Acceleration from previous loop - prevRioAx = rioAx; - prevRioAy = rioAy; - prevRioAz = rioAz; - - // IMU rotate is also compute-only (already primitives) - final double imuAx = (imuCos * imuInputs.linearAccelX - imuSin * imuInputs.linearAccelY); - final double imuAy = (imuSin * imuInputs.linearAccelX + imuCos * imuInputs.linearAccelY); - final double imuAz = imuInputs.linearAccelZ; - - final double imuJx = (imuCos * imuInputs.jerkX - imuSin * imuInputs.jerkY); - final double imuJy = (imuSin * imuInputs.jerkX + imuCos * imuInputs.jerkY); - final double imuJz = imuInputs.jerkZ; - - // Fill accel arrays (still math) - rioAccelArr[0] = rioAx; - rioAccelArr[1] = rioAy; - rioAccelArr[2] = rioAz; - imuAccelArr[0] = imuAx; - imuAccelArr[1] = imuAy; - imuAccelArr[2] = imuAz; + prevRioAcc = rioAcc; - final boolean doHeavyLogs = (++loopCount >= LOG_EVERY_N); - if (doHeavyLogs) { - loopCount = 0; - rioJerkArr[0] = rioJx; - rioJerkArr[1] = rioJy; - rioJerkArr[2] = rioJz; - imuJerkArr[0] = imuJx; - imuJerkArr[1] = imuJy; - imuJerkArr[2] = imuJz; - } + // IMU accelerations and jerks + imuAcc = imuInputs.linearAccel.rotateBy(RobotConstants.kIMUOrientation); // Logging - Logger.recordOutput("Accel/Rio/Accel_mps2", rioAccelArr); - Logger.recordOutput("Accel/IMU/Accel_mps2", imuAccelArr); + Logger.recordOutput("Accel/Rio/Accel_mps2", rioAcc); + Logger.recordOutput("Accel/IMU/Accel_mps2", imuAcc); + // Every N loops, compute and log the Jerk + final boolean doHeavyLogs = (++loopCount >= LOG_EVERY_N); if (doHeavyLogs) { - Logger.recordOutput("Accel/Rio/Jerk_mps3", rioJerkArr); - Logger.recordOutput("Accel/IMU/Jerk_mps3", imuJerkArr); + loopCount = 0; + rioJerk = rioAcc.minus(prevRioAcc).div(Constants.loopPeriodSecs); + imuJerk = imuInputs.linearJerk.rotateBy(RobotConstants.kIMUOrientation); + Logger.recordOutput("Accel/Rio/Jerk_mps3", rioJerk); + Logger.recordOutput("Accel/IMU/Jerk_mps3", imuJerk); final double[] ts = imuInputs.odometryYawTimestamps; if (ts.length > 0) { diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java index 2a7a98f..f5f6315 100644 --- a/src/main/java/frc/robot/subsystems/imu/Imu.java +++ b/src/main/java/frc/robot/subsystems/imu/Imu.java @@ -19,8 +19,9 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; +import frc.robot.util.VirtualSubsystem; -public class Imu { +public class Imu extends VirtualSubsystem { private final ImuIO io; private final ImuIO.ImuIOInputs inputs = new ImuIO.ImuIOInputs(); @@ -30,11 +31,13 @@ public class Imu { private Translation3d cachedAccel = Translation3d.kZero; private Translation3d cachedJerk = Translation3d.kZero; + // Constructor public Imu(ImuIO io) { this.io = io; } - public void periodic() { + // Periodic function to read inputs + public void rbsiPeriodic() { io.updateInputs(inputs); } @@ -70,12 +73,11 @@ private void refreshCachesIfNeeded() { cacheStampNs = stamp; cachedYaw = Rotation2d.fromRadians(inputs.yawPositionRad); - cachedAccel = new Translation3d(inputs.linearAccelX, inputs.linearAccelY, inputs.linearAccelZ); - cachedJerk = new Translation3d(inputs.jerkX, inputs.jerkY, inputs.jerkZ); + cachedAccel = inputs.linearAccel; + cachedJerk = inputs.linearJerk; } // ---------------- SIM PUSH (primitive-only boundary) ---------------- - /** Simulation: push authoritative yaw (radians) into the IO layer */ public void simulationSetYawRad(double yawRad) { io.simulationSetYawRad(yawRad); diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index 37aa695..bf350c4 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -17,6 +17,7 @@ package frc.robot.subsystems.imu; +import edu.wpi.first.math.geometry.Translation3d; import frc.robot.util.RBSIIO; import org.littletonrobotics.junction.AutoLog; @@ -43,16 +44,10 @@ class ImuIOInputs { public double yawRateRadPerSec = 0.0; /** Linear acceleration in robot frame (m/s^2) */ - public double linearAccelX = 0.0; - - public double linearAccelY = 0.0; - public double linearAccelZ = 0.0; + public Translation3d linearAccel = Translation3d.kZero; /** Linear jerk in robot frame (m/s^3) */ - public double jerkX = 0.0; - - public double jerkY = 0.0; - public double jerkZ = 0.0; + public Translation3d linearJerk = Translation3d.kZero; /** Time spent in the IO update call (seconds) */ public double latencySeconds = 0.0; diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java index 4bf1edc..18fa3c2 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java @@ -19,6 +19,7 @@ import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.subsystems.drive.PhoenixOdometryThread; @@ -38,7 +39,7 @@ public class ImuIONavX implements ImuIO { private final Queue yawTimestampQueue; // Previous accel (m/s^2) for jerk - private double prevAx = 0.0, prevAy = 0.0, prevAz = 0.0; + private Translation3d prevAcc = Translation3d.kZero; private long prevTimestampNs = 0L; // Reusable buffers for queue drain @@ -82,29 +83,23 @@ public void updateInputs(ImuIOInputs inputs) { inputs.yawRateRadPerSec = yawRateDegPerSec * DEG_TO_RAD; // World linear accel (NavX returns "g" typically). Convert to m/s^2. - final double ax = navx.getWorldLinearAccelX() * G_TO_MPS2; - final double ay = navx.getWorldLinearAccelY() * G_TO_MPS2; - final double az = navx.getWorldLinearAccelZ() * G_TO_MPS2; - - inputs.linearAccelX = ax; - inputs.linearAccelY = ay; - inputs.linearAccelZ = az; + inputs.linearAccel = + new Translation3d( + navx.getWorldLinearAccelX(), + navx.getWorldLinearAccelY(), + navx.getWorldLinearAccelZ()) + .times(G_TO_MPS2); // Jerk if (prevTimestampNs != 0L) { final double dt = (start - prevTimestampNs) * 1e-9; if (dt > 1e-6) { - final double invDt = 1.0 / dt; - inputs.jerkX = (ax - prevAx) * invDt; - inputs.jerkY = (ay - prevAy) * invDt; - inputs.jerkZ = (az - prevAz) * invDt; + inputs.linearJerk = inputs.linearAccel.minus(prevAcc).div(dt); } } prevTimestampNs = start; - prevAx = ax; - prevAy = ay; - prevAz = az; + prevAcc = inputs.linearAccel; inputs.timestampNs = start; @@ -126,6 +121,11 @@ public void updateInputs(ImuIOInputs inputs) { inputs.latencySeconds = (end - start) * 1e-9; } + /** + * Zero the YAW to this radian value + * + * @param yawRad The radian value to which to zero + */ @Override public void zeroYawRad(double yawRad) { navx.setAngleAdjustment(yawRad / DEG_TO_RAD); @@ -133,7 +133,7 @@ public void zeroYawRad(double yawRad) { // Reset jerk history so you don't spike on the next frame prevTimestampNs = 0L; - prevAx = prevAy = prevAz = 0.0; + prevAcc = Translation3d.kZero; } private int drainOdomQueues() { @@ -174,25 +174,4 @@ private void ensureOdomCapacity(int n) { odomTsBuf = new double[newCap]; odomYawRadBuf = new double[newCap]; } - - // /** - // * Zero the NavX - // * - // *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, - // * however, shows that it's not doing what I think it should be doing. There is likely - // * interference with something else in the odometry - // */ - // @Override - // public void zero() { - // // With the Pigeon facing forward, forward depends on the alliance selected. - // // Set Angle Adjustment based on alliance - // if (DriverStation.getAlliance().get() == Alliance.Blue) { - // navx.setAngleAdjustment(0.0); - // } else { - // navx.setAngleAdjustment(180.0); - // } - // System.out.println("Setting YAW to " + navx.getAngleAdjustment()); - // navx.zeroYaw(); - // } - } diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java index 0e07f02..0db40ef 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java @@ -22,6 +22,8 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Translation3d; +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; @@ -31,12 +33,13 @@ import java.util.Iterator; import java.util.Queue; -/** IMU IO for CTRE Pigeon2 (primitive-only hot path) */ +/** IMU IO for CTRE Pigeon2 */ public class ImuIOPigeon2 implements ImuIO { - private static final double DEG_TO_RAD = Math.PI / 180.0; + // Constants private static final double G_TO_MPS2 = 9.80665; + // Define the Pigeon2 private final Pigeon2 pigeon = new Pigeon2( SwerveConstants.kPigeonId, RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName)); @@ -53,13 +56,14 @@ public class ImuIOPigeon2 implements ImuIO { private final Queue odomYawsDeg; // Previous accel for jerk (m/s^2) - private double prevAx = 0.0, prevAy = 0.0, prevAz = 0.0; + private Translation3d prevAcc = Translation3d.kZero; private long prevTimestampNs = 0L; // Reusable buffers for queue-drain (avoid streams) private double[] odomTsBuf = new double[8]; private double[] odomYawRadBuf = new double[8]; + // Constructor public ImuIOPigeon2() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); @@ -77,6 +81,7 @@ public ImuIOPigeon2() { odomYawsDeg = PhoenixOdometryThread.getInstance().registerSignal(yawSignal); } + /** Update the Inputs */ @Override public void updateInputs(ImuIOInputs inputs) { final long start = System.nanoTime(); @@ -88,33 +93,25 @@ public void updateInputs(ImuIOInputs inputs) { final double yawDeg = yawSignal.getValueAsDouble(); final double yawRateDegPerSec = yawRateSignal.getValueAsDouble(); - inputs.yawPositionRad = yawDeg * DEG_TO_RAD; - inputs.yawRateRadPerSec = yawRateDegPerSec * DEG_TO_RAD; + inputs.yawPositionRad = Units.degreesToRadians(yawDeg); + inputs.yawRateRadPerSec = Units.degreesToRadians(yawRateDegPerSec); // Accel: Phoenix returns "g" for these signals (common for Pigeon2). Convert to m/s^2 - final double ax = accelX.getValueAsDouble() * G_TO_MPS2; - final double ay = accelY.getValueAsDouble() * G_TO_MPS2; - final double az = accelZ.getValueAsDouble() * G_TO_MPS2; - - inputs.linearAccelX = ax; - inputs.linearAccelY = ay; - inputs.linearAccelZ = az; + inputs.linearAccel = + new Translation3d( + accelX.getValueAsDouble(), accelY.getValueAsDouble(), accelZ.getValueAsDouble()) + .times(G_TO_MPS2); // Jerk from delta accel / dt if (prevTimestampNs != 0L) { final double dt = (start - prevTimestampNs) * 1e-9; if (dt > 1e-6) { - final double invDt = 1.0 / dt; - inputs.jerkX = (ax - prevAx) * invDt; - inputs.jerkY = (ay - prevAy) * invDt; - inputs.jerkZ = (az - prevAz) * invDt; + inputs.linearJerk = inputs.linearAccel.minus(prevAcc).div(dt); } } prevTimestampNs = start; - prevAx = ax; - prevAy = ay; - prevAz = az; + prevAcc = inputs.linearAccel; inputs.timestampNs = start; @@ -136,9 +133,14 @@ public void updateInputs(ImuIOInputs inputs) { inputs.latencySeconds = (end - start) * 1e-9; } + /** + * Zero the YAW to this radian value + * + * @param yawRad The radian value to which to zero + */ @Override public void zeroYawRad(double yawRad) { - pigeon.setYaw(yawRad / DEG_TO_RAD); + pigeon.setYaw(Units.radiansToDegrees(yawRad)); } private int drainOdometryQueuesIntoBuffers() { @@ -160,7 +162,7 @@ private int drainOdometryQueuesIntoBuffers() { int i = 0; while (i < n && itT.hasNext() && itY.hasNext()) { odomTsBuf[i] = itT.next(); - odomYawRadBuf[i] = itY.next() * DEG_TO_RAD; + odomYawRadBuf[i] = Units.degreesToRadians(itY.next()); i++; } diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index 080dec1..26820d4 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -17,12 +17,13 @@ package frc.robot.subsystems.imu; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import org.littletonrobotics.junction.Logger; -/** Simulated IMU for full robot simulation & replay logging (primitive-only) */ +/** Simulated IMU for full robot simulation & replay logging */ public class ImuIOSim implements ImuIO { - private static final double RAD_TO_DEG = 180.0 / Math.PI; // --- AUTHORITATIVE SIM STATE (PRIMITIVES) --- private double yawRad = 0.0; @@ -39,7 +40,6 @@ public class ImuIOSim implements ImuIO { public ImuIOSim() {} // ---------------- SIMULATION INPUTS (PUSH) ---------------- - @Override public void simulationSetYawRad(double yawRad) { this.yawRad = yawRad; @@ -58,7 +58,6 @@ public void simulationSetLinearAccelMps2(double ax, double ay, double az) { } // ---------------- IO UPDATE (PULL) ---------------- - @Override public void updateInputs(ImuIOInputs inputs) { inputs.connected = true; @@ -67,15 +66,11 @@ public void updateInputs(ImuIOInputs inputs) { // Authoritative sim state inputs.yawPositionRad = yawRad; inputs.yawRateRadPerSec = yawRateRadPerSec; - inputs.linearAccelX = ax; - inputs.linearAccelY = ay; - inputs.linearAccelZ = az; + inputs.linearAccel = new Translation3d(ax, ay, az); // Jerk: SIM doesn’t have a prior accel here unless you want it; set to 0 by default. // If you do want jerk, you can add prevAx/prevAy/prevAz + dt just like the real IO. - inputs.jerkX = 0.0; - inputs.jerkY = 0.0; - inputs.jerkZ = 0.0; + inputs.linearJerk = Translation3d.kZero; // Maintain odometry history pushOdomSample(Timer.getFPGATimestamp(), yawRad); @@ -101,10 +96,15 @@ public void updateInputs(ImuIOInputs inputs) { // Optional: SIM logging (primitive-friendly) Logger.recordOutput("IMU/YawRad", yawRad); - Logger.recordOutput("IMU/YawDeg", yawRad * RAD_TO_DEG); - Logger.recordOutput("IMU/YawRateDps", yawRateRadPerSec * RAD_TO_DEG); + Logger.recordOutput("IMU/YawDeg", Units.radiansToDegrees(yawRad)); + Logger.recordOutput("IMU/YawRateDps", Units.radiansToDegrees(yawRateRadPerSec)); } + /** + * Zero the YAW to this radian value + * + * @param yawRad The radian value to which to zero + */ @Override public void zeroYawRad(double yawRad) { this.yawRad = yawRad; From 259635be757cd3ae74de91d5969c00f92c35e3d2 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 2 Feb 2026 22:18:17 -0700 Subject: [PATCH 2/6] Clean up Drive.java --- src/main/java/frc/robot/Constants.java | 3 + src/main/java/frc/robot/RobotContainer.java | 3 +- .../frc/robot/subsystems/drive/Drive.java | 449 +++++++++--------- 3 files changed, 233 insertions(+), 222 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5b76500..16f185b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -323,6 +323,9 @@ public static final class DrivebaseConstants { SwerveConstants.kDriveGearRatio / DCMotor.getKrakenX60Foc(1).KtNMPerAmp; public static final double kSteerP = 400.0; public static final double kSteerD = 20.0; + + // Odometry-related constants + public static final double kHistorySize = 1.5; // seconds } /************************************************************************* */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9c5a167..5c175d3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,6 +34,7 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.CANBuses; +import frc.robot.Constants.Cameras; import frc.robot.Constants.OperatorConstants; import frc.robot.FieldConstants.AprilTagLayoutType; import frc.robot.commands.AutopilotCommands; @@ -167,7 +168,7 @@ public RobotContainer() { m_flywheel = new Flywheel(new FlywheelIOSim()); // ---------------- Vision IOs (robot code) ---------------- - var cams = frc.robot.Constants.Cameras.ALL; + var cams = Cameras.ALL; m_vision = new Vision( m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsSim(m_drivebase)); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 46788c2..0046db7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -60,25 +60,33 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +/** + * Drive subsystem (RBSISubsystem) + * + *

The Drive subsystem controls the individual swerve Modules and owns the odometry of the robot. + * The odometry is updated from both the swerve modules and (optionally) the vision subsystem. + */ public class Drive extends RBSISubsystem { - // --- buffers for time-aligned pose + yaw + yawRate history --- - private final ConcurrentTimeInterpolatableBuffer poseBuffer = - frc.robot.util.ConcurrentTimeInterpolatableBuffer.createBuffer(1.5); + // Declare Hardware + private final Imu imu; + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId; + // Buffers for necessary things + private final ConcurrentTimeInterpolatableBuffer poseBuffer = + ConcurrentTimeInterpolatableBuffer.createBuffer(DrivebaseConstants.kHistorySize); private final ConcurrentTimeInterpolatableBuffer yawBuffer = - frc.robot.util.ConcurrentTimeInterpolatableBuffer.createDoubleBuffer(1.5); - + ConcurrentTimeInterpolatableBuffer.createDoubleBuffer(DrivebaseConstants.kHistorySize); private final ConcurrentTimeInterpolatableBuffer yawRateBuffer = - frc.robot.util.ConcurrentTimeInterpolatableBuffer.createDoubleBuffer(1.5); + ConcurrentTimeInterpolatableBuffer.createDoubleBuffer(DrivebaseConstants.kHistorySize); - static final Lock odometryLock = new ReentrantLock(); - private final Imu imu; - private final Module[] modules = new Module[4]; // FL, FR, BL, BR - private final SysIdRoutine sysId; + // Declare an alert private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); + // Declare odometry and pose-related variables + static final Lock odometryLock = new ReentrantLock(); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { @@ -87,15 +95,24 @@ public class Drive extends RBSISubsystem { new SwerveModulePosition(), new SwerveModulePosition() }; - private final SwerveModulePosition[] odomPositions = new SwerveModulePosition[4]; + private final SwerveModulePosition[] odomPositions = { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; private SwerveDrivePoseEstimator m_PoseEstimator = new SwerveDrivePoseEstimator(kinematics, Rotation2d.kZero, lastModulePositions, Pose2d.kZero); + // Declare PID controller and siumulation physics private ProfiledPIDController angleController; - private DriveSimPhysics simPhysics; - // Constructor + // Pose reset gate (vision + anything latency-sensitive) + private volatile long poseResetEpoch = 0; // monotonic counter + private volatile double lastPoseResetTimestamp = Double.NEGATIVE_INFINITY; + + /** Constructor */ public Drive(Imu imu) { this.imu = imu; @@ -151,8 +168,8 @@ public Drive(Imu imu) { default: throw new RuntimeException("Invalid Swerve Drive Type"); } - // Start odometry thread (for the real robot) + // Start odometry thread (for the real robot) PhoenixOdometryThread.getInstance().start(); } else { @@ -211,12 +228,16 @@ public Drive(Imu imu) { break; case CHOREO: - // TODO: Probably need to add something here for Choreo autonomous path building + // TODO: If your team is using Choreo, you'll know what to do here... + break; + + case MANUAL: + // Nothing to be done for MANUAL; may just use AutoPilot break; default: } - // Configure SysId + // Configure SysId for drivebase characterization sysId = new SysIdRoutine( new SysIdRoutine.Config( @@ -228,126 +249,164 @@ public Drive(Imu imu) { (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); } + /************************************************************************* */ /** Periodic function that is called each robot cycle by the command scheduler */ @Override public void rbsiPeriodic() { odometryLock.lock(); try { - // Get the IMU inputs + // Ensure IMU inputs are fresh for this cycle final var imuInputs = imu.getInputs(); // Stop modules & log empty setpoint states if disabled if (DriverStation.isDisabled()) { - for (var module : modules) module.stop(); + for (var module : modules) { + module.stop(); + } Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } - // Module periodic updates, which drains queues this cycle - for (var module : modules) module.periodic(); + // Drain per-module odometry queues for this cycle + for (var module : modules) { + module.periodic(); + } - // Feed historical samples into odometry if REAL robot + // -------- REAL ODOMETRY REPLAY (canonical timestamps from module[0]) -------- if (Constants.getMode() != Mode.SIM) { + final double[] ts = modules[0].getOdometryTimestamps(); + final int n = (ts == null) ? 0 : ts.length; - // ---- Gather per-module histories ---- - final double[][] perModuleTs = new double[4][]; - final SwerveModulePosition[][] perModulePos = new SwerveModulePosition[4][]; - int unionCap = 0; - - for (int m = 0; m < 4; m++) { - perModuleTs[m] = modules[m].getOdometryTimestamps(); - perModulePos[m] = modules[m].getOdometryPositions(); - unionCap += perModuleTs[m].length; - } + if (n == 0) { + // Still keep yaw buffers updated for yaw-gating callers + double now = Timer.getFPGATimestamp(); + yawBuffer.addSample(now, imuInputs.yawPositionRad); + yawRateBuffer.addSample(now, imuInputs.yawRateRadPerSec); - if (unionCap == 0) { gyroDisconnectedAlert.set(!imuInputs.connected); return; } - // ---- Fill yaw buffer from IMU odometry samples (preferred) ---- - // These timestamps are authoritative for yaw interpolation and yaw-rate gating. - if (imuInputs.connected - && imuInputs.odometryYawTimestamps != null - && imuInputs.odometryYawPositionsRad != null - && imuInputs.odometryYawTimestamps.length == imuInputs.odometryYawPositionsRad.length - && imuInputs.odometryYawTimestamps.length > 0) { - - final double[] yts = imuInputs.odometryYawTimestamps; - final double[] yaws = imuInputs.odometryYawPositionsRad; - - for (int i = 0; i < yts.length; i++) { - yawBuffer.addSample(yts[i], yaws[i]); - if (i > 0) { - double dt = yts[i] - yts[i - 1]; - if (dt > 1e-6) { - yawRateBuffer.addSample(yts[i], (yaws[i] - yaws[i - 1]) / dt); - } + // Optional IMU odometry yaw queue (preferred if aligned) + final boolean hasYawQueue = + imuInputs.connected + && imuInputs.odometryYawTimestamps != null + && imuInputs.odometryYawPositionsRad != null + && imuInputs.odometryYawTimestamps.length + == imuInputs.odometryYawPositionsRad.length + && imuInputs.odometryYawTimestamps.length > 0; + + final double[] yawTs = hasYawQueue ? imuInputs.odometryYawTimestamps : null; + final double[] yawPos = hasYawQueue ? imuInputs.odometryYawPositionsRad : null; + + // Replay each odometry sample + for (int i = 0; i < n; i++) { + // Build module positions at this sample index + for (int m = 0; m < 4; m++) { + SwerveModulePosition[] hist = modules[m].getOdometryPositions(); + + // Defensive: if one module returned fewer samples, clamp to last available + if (hist == null || hist.length == 0) { + odomPositions[m] = modules[m].getPosition(); + } else if (i < hist.length) { + odomPositions[m] = hist[i]; + } else { + odomPositions[m] = hist[hist.length - 1]; } } - } else { - // fallback: buffer "now" yaw only - double now = Timer.getFPGATimestamp(); - yawBuffer.addSample(now, imuInputs.yawPositionRad); - yawRateBuffer.addSample(now, imuInputs.yawRateRadPerSec); - } - - // ---- Build union timeline ---- - final double[] unionTs = new double[Math.max(unionCap, 1)]; - final double EPS = 1e-4; // 0.1 ms - final int unionN = buildUnionTimeline(perModuleTs, unionTs, EPS); - // ---- Replay at union times ---- - for (int i = 0; i < unionN; i++) { - final double t = unionTs[i]; - - // Interpolate each module to union time - for (int m = 0; m < 4; m++) { - SwerveModulePosition p = interpolateModulePosition(perModuleTs[m], perModulePos[m], t); - odomPositions[m] = p; + // Determine yaw at this timestamp + final double t = ts[i]; + double yawRad = imuInputs.yawPositionRad; // fallback + + if (hasYawQueue) { + // If yaw queue is index-aligned with module timestamps, use by index. + // Otherwise, fall back to interpolating from yawBuffer. + if (yawTs.length > i && Math.abs(yawTs[i] - t) < 1e-3) { + yawRad = yawPos[i]; + + // Keep yaw/yawRate buffers updated in the same timebase for gating queries + yawBuffer.addSample(t, yawRad); + if (i > 0 && yawTs.length > (i - 1)) { + double dt = yawTs[i] - yawTs[i - 1]; + if (dt > 1e-6) { + yawRateBuffer.addSample(t, (yawPos[i] - yawPos[i - 1]) / dt); + } + } + } else { + // Not index-aligned; build yaw buffer from queue once, then sample it + // (cheap enough; queue is short) + for (int k = 0; k < yawTs.length; k++) { + yawBuffer.addSample(yawTs[k], yawPos[k]); + if (k > 0) { + double dt = yawTs[k] - yawTs[k - 1]; + if (dt > 1e-6) { + yawRateBuffer.addSample(yawTs[k], (yawPos[k] - yawPos[k - 1]) / dt); + } + } + } + yawRad = yawBuffer.getSample(t).orElse(imuInputs.yawPositionRad); + } + } else { + // No yaw queue: store "now" primitives for gating users (still useful) + double now = Timer.getFPGATimestamp(); + yawBuffer.addSample(now, imuInputs.yawPositionRad); + yawRateBuffer.addSample(now, imuInputs.yawRateRadPerSec); } - // Interpolate yaw at union time - final double yawRad = yawBuffer.getSample(t).orElse(imuInputs.yawPositionRad); - + // Feed estimator at this historical timestamp m_PoseEstimator.updateWithTime(t, Rotation2d.fromRadians(yawRad), odomPositions); - // keep buffer in the same timebase as estimator + // Maintain pose history in the SAME timebase as estimator poseBuffer.addSample(t, m_PoseEstimator.getEstimatedPosition()); } Logger.recordOutput("Drive/Pose", m_PoseEstimator.getEstimatedPosition()); + gyroDisconnectedAlert.set(!imuInputs.connected); + return; } - gyroDisconnectedAlert.set(!imuInputs.connected && Constants.getMode() != Mode.SIM); + // SIMULATION: Keep sim pose buffer time-aligned, too + double now = Timer.getFPGATimestamp(); + poseBuffer.addSample(now, simPhysics.getPose()); + yawBuffer.addSample(now, simPhysics.getYaw().getRadians()); + yawRateBuffer.addSample(now, simPhysics.getOmegaRadPerSec()); + + Logger.recordOutput("Drive/Pose", simPhysics.getPose()); + gyroDisconnectedAlert.set(false); } finally { odometryLock.unlock(); } } - /** Simulation Periodic Method */ + /** + * Simulation Periodic Method + * + *

This function runs only for simulation, but does similar processing to the REAL periodic + * function. Instead of reading back what the modules actually say, use physics to predict where + * the module would have gone. + */ @Override public void simulationPeriodic() { final double dt = Constants.loopPeriodSecs; - // 1) Advance module wheel physics + // Advance module wheel physics for (int i = 0; i < modules.length; i++) { modules[i].simulationPeriodic(); } - // 2) Get module states from modules (authoritative) - NO STREAMS + // Get module states from modules final SwerveModuleState[] moduleStates = new SwerveModuleState[modules.length]; for (int i = 0; i < modules.length; i++) { moduleStates[i] = modules[i].getState(); } - // 3) Update SIM physics (linear + angular) + // Update SIM physics (linear & angular motion of the robot) simPhysics.update(moduleStates, dt); - // 4) Feed IMU from authoritative physics (primitive-only boundary) - final double yawRad = - simPhysics.getYaw().getRadians(); // or simPhysics.getYawRad() if you have it + // Feed the simulated IMU from authoritative physics + final double yawRad = simPhysics.getYaw().getRadians(); final double omegaRadPerSec = simPhysics.getOmegaRadPerSec(); final double ax = simPhysics.getLinearAccel().getX(); @@ -357,17 +416,15 @@ public void simulationPeriodic() { imu.simulationSetOmegaRadPerSec(omegaRadPerSec); imu.simulationSetLinearAccelMps2(ax, ay, 0.0); - // 5) Feed PoseEstimator with authoritative yaw and module positions - // (PoseEstimator still wants objects -> boundary conversion stays here) + // Feed PoseEstimator with authoritative yaw and module positions final SwerveModulePosition[] modulePositions = new SwerveModulePosition[modules.length]; for (int i = 0; i < modules.length; i++) { modulePositions[i] = modules[i].getPosition(); } - m_PoseEstimator.resetPosition( Rotation2d.fromRadians(yawRad), modulePositions, simPhysics.getPose()); - // 6) Optional: inject vision measurement in SIM + // If simulated vision available, inject vision measurement if (simulatedVisionAvailable) { final Pose2d visionPose = getSimulatedVisionPose(); final double visionTimestamp = Timer.getFPGATimestamp(); @@ -377,7 +434,7 @@ public void simulationPeriodic() { poseBuffer.addSample(Timer.getFPGATimestamp(), simPhysics.getPose()); - // 7) Logging + // Logging Logger.recordOutput("Sim/Pose", simPhysics.getPose()); Logger.recordOutput("Sim/YawRad", yawRad); Logger.recordOutput("Sim/OmegaRadPerSec", simPhysics.getOmegaRadPerSec()); @@ -399,7 +456,7 @@ public void setMotorBrake(boolean brake) { } } - /** Stops the drive. */ + /** Stop the drive. */ public void stop() { runVelocity(new ChassisSpeeds()); } @@ -513,37 +570,6 @@ public Pose2d getPose() { return m_PoseEstimator.getEstimatedPosition(); } - /** Returns interpolated odometry pose at a given timestamp. */ - public Optional getPoseAtTime(double timestampSeconds) { - return poseBuffer.getSample(timestampSeconds); - } - - /** Adds a vision measurement safely into the PoseEstimator. */ - public void addVisionMeasurement(Pose2d pose, double timestampSeconds, Matrix stdDevs) { - odometryLock.lock(); - try { - m_PoseEstimator.addVisionMeasurement(pose, timestampSeconds, stdDevs); - } finally { - odometryLock.unlock(); - } - } - - /** Max abs yaw rate over [t0, t1] using buffered yaw-rate history (no streams). */ - public OptionalDouble getMaxAbsYawRateRadPerSec(double t0, double t1) { - if (t1 < t0) return OptionalDouble.empty(); - var sub = yawRateBuffer.getInternalBuffer().subMap(t0, true, t1, true); - if (sub.isEmpty()) return OptionalDouble.empty(); - - double maxAbs = 0.0; - boolean any = false; - for (double v : sub.values()) { - any = true; - double a = Math.abs(v); - if (a > maxAbs) maxAbs = a; - } - return any ? OptionalDouble.of(maxAbs) : OptionalDouble.empty(); - } - /** Returns the current odometry rotation. */ @AutoLogOutput(key = "Odometry/Yaw") public Rotation2d getHeading() { @@ -553,27 +579,8 @@ public Rotation2d getHeading() { return imu.getYaw(); } - /** Returns an array of module translations. */ - public static Translation2d[] getModuleTranslations() { - return new Translation2d[] { - new Translation2d(kFLXPosMeters, kFLYPosMeters), - new Translation2d(kFRXPosMeters, kFRYPosMeters), - new Translation2d(kBLXPosMeters, kBLYPosMeters), - new Translation2d(kBRXPosMeters, kBRYPosMeters) - }; - } - - /** Returns the position of each module in radians. */ - public double[] getWheelRadiusCharacterizationPositions() { - double[] values = new double[4]; - for (int i = 0; i < 4; i++) { - values[i] = modules[i].getWheelRadiusCharacterizationPosition(); - } - return values; - } - /** - * Returns the measured chassis speeds in FIELD coordinates. + * Returns the measured chassis speeds of the modules in FIELD coordinates. * *

+X = field forward +Y = field left CCW+ = counterclockwise */ @@ -581,7 +588,6 @@ public double[] getWheelRadiusCharacterizationPositions() { public ChassisSpeeds getFieldRelativeSpeeds() { // Robot-relative measured speeds from modules ChassisSpeeds robotRelative = getChassisSpeeds(); - // Convert to field-relative using authoritative yaw return ChassisSpeeds.fromRobotRelativeSpeeds(robotRelative, getHeading()); } @@ -597,13 +603,45 @@ public Translation2d getFieldLinearVelocity() { return new Translation2d(fieldSpeeds.vxMetersPerSecond, fieldSpeeds.vyMetersPerSecond); } - /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ - public double getFFCharacterizationVelocity() { - double output = 0.0; - for (int i = 0; i < 4; i++) { - output += modules[i].getFFCharacterizationVelocity() / 4.0; + /** Returns interpolated odometry pose at a given timestamp. */ + public Optional getPoseAtTime(double timestampSeconds) { + return poseBuffer.getSample(timestampSeconds); + } + + /** + * Max abs yaw rate over [t0, t1] using buffered yaw-rate history + * + * @param t0 Interval start + * @param t1 interval end + * @return Maximum yaw rate + */ + public OptionalDouble getMaxAbsYawRateRadPerSec(double t0, double t1) { + // If end before start, return empty + if (t1 < t0) return OptionalDouble.empty(); + + // Get the subset of entries from the buffer + var sub = yawRateBuffer.getInternalBuffer().subMap(t0, true, t1, true); + if (sub.isEmpty()) return OptionalDouble.empty(); + + double maxAbs = 0.0; + boolean any = false; + for (double v : sub.values()) { + any = true; + double a = Math.abs(v); + if (a > maxAbs) maxAbs = a; } - return output; + // Return a value if there's anything to report, else empty + return any ? OptionalDouble.of(maxAbs) : OptionalDouble.empty(); + } + + /** Get the last EPOCH of a pose reset */ + public long getPoseResetEpoch() { + return poseResetEpoch; + } + + /** Get the last TIMESTAMP of a pose reset */ + public double getLastPoseResetTimestamp() { + return lastPoseResetTimestamp; } /** Returns the maximum linear speed in meters per sec. */ @@ -626,6 +664,34 @@ public double getMaxAngularAccelRadPerSecPerSec() { return getMaxLinearAccelMetersPerSecPerSec() / kDriveBaseRadiusMeters; } + /** Returns an array of module translations. */ + public static Translation2d[] getModuleTranslations() { + return new Translation2d[] { + new Translation2d(kFLXPosMeters, kFLYPosMeters), + new Translation2d(kFRXPosMeters, kFRYPosMeters), + new Translation2d(kBLXPosMeters, kBLYPosMeters), + new Translation2d(kBRXPosMeters, kBRYPosMeters) + }; + } + + /** Returns the position of each module in radians. */ + public double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += modules[i].getFFCharacterizationVelocity() / 4.0; + } + return output; + } + /* Setter Functions ****************************************************** */ /** Resets the current odometry pose. */ @@ -651,18 +717,21 @@ public void zeroHeading() { markPoseReset(Timer.getFPGATimestamp()); } - // ---- Pose reset gate (vision + anything latency-sensitive) ---- - private volatile long poseResetEpoch = 0; // monotonic counter - private volatile double lastPoseResetTimestamp = Double.NEGATIVE_INFINITY; - - public long getPoseResetEpoch() { - return poseResetEpoch; - } - - public double getLastPoseResetTimestamp() { - return lastPoseResetTimestamp; + /** Adds a vision measurement safely into the PoseEstimator. */ + public void addVisionMeasurement(Pose2d pose, double timestampSeconds, Matrix stdDevs) { + odometryLock.lock(); + try { + m_PoseEstimator.addVisionMeasurement(pose, timestampSeconds, stdDevs); + } finally { + odometryLock.unlock(); + } } + /** + * Sets the EPOCH and TIMESTAMP for a pose reset + * + * @param fpgaNow The FPGA timestamp of the pose reset + */ private void markPoseReset(double fpgaNow) { lastPoseResetTimestamp = fpgaNow; poseResetEpoch++; @@ -670,7 +739,10 @@ private void markPoseReset(double fpgaNow) { Logger.recordOutput("Drive/PoseResetTimestamp", lastPoseResetTimestamp); } + /** UTILITY FUNCTION SECTION ********************************************* */ + /** CHOREO SECTION (Ignore if AutoType == PATHPLANNER) ******************* */ + /** Choreo: Reset odometry */ public Command resetOdometry(Pose2d orElseGet) { // TODO Auto-generated method stub @@ -725,7 +797,7 @@ public void followTrajectory(SwerveSample sample) { runVelocity(speeds); } - // ---------------- SIM VISION ---------------- + /** SIMULATION VISION FUNCTIONS ****************************************** */ // Vision measurement enabled in simulation private boolean simulatedVisionAvailable = true; @@ -764,69 +836,4 @@ private edu.wpi.first.math.Matrix getSimulatedVisionStdDevs() { stdDevs.set(2, 0, Math.toRadians(2)); // rotation standard deviation (radians) return stdDevs; } - - private static SwerveModulePosition interpolateModulePosition( - double[] ts, SwerveModulePosition[] samples, double t) { - - final int n = ts.length; - if (n == 0) return new SwerveModulePosition(); - - if (t <= ts[0]) return samples[0]; - if (t >= ts[n - 1]) return samples[n - 1]; - - int lo = 0, hi = n - 1; - while (lo < hi) { - int mid = (lo + hi) >>> 1; - if (ts[mid] < t) lo = mid + 1; - else hi = mid; - } - int i1 = lo; - int i0 = i1 - 1; - - double t0 = ts[i0]; - double t1 = ts[i1]; - if (t1 <= t0) return samples[i1]; - - double alpha = (t - t0) / (t1 - t0); - - double dist = - samples[i0].distanceMeters - + (samples[i1].distanceMeters - samples[i0].distanceMeters) * alpha; - - Rotation2d angle = samples[i0].angle.interpolate(samples[i1].angle, alpha); - - return new SwerveModulePosition(dist, angle); - } - - private static int buildUnionTimeline(double[][] perModuleTs, double[] outUnion, double epsSec) { - int[] idx = new int[perModuleTs.length]; - int outN = 0; - - while (true) { - double minT = Double.POSITIVE_INFINITY; - boolean any = false; - - for (int m = 0; m < perModuleTs.length; m++) { - double[] ts = perModuleTs[m]; - if (idx[m] >= ts.length) continue; - any = true; - minT = Math.min(minT, ts[idx[m]]); - } - - if (!any) break; - - if (outN == 0 || Math.abs(minT - outUnion[outN - 1]) > epsSec) { - outUnion[outN++] = minT; - } - - for (int m = 0; m < perModuleTs.length; m++) { - double[] ts = perModuleTs[m]; - while (idx[m] < ts.length && Math.abs(ts[idx[m]] - minT) <= epsSec) { - idx[m]++; - } - } - } - - return outN; - } } From 5dac0e58ca4e3b62e32afd90798253e349c495ec Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 2 Feb 2026 22:33:13 -0700 Subject: [PATCH 3/6] Clean the Moudule modules a bit --- src/main/java/frc/robot/RobotContainer.java | 16 +++++--- .../subsystems/drive/DriveSimPhysics.java | 5 ++- .../frc/robot/subsystems/drive/Module.java | 38 +++++++++++++++---- .../frc/robot/subsystems/drive/ModuleIO.java | 1 + .../subsystems/drive/ModuleIOTalonFX.java | 4 +- 5 files changed, 48 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5c175d3..fc2643d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,9 +3,15 @@ // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage // -// Use of this source code is governed by a BSD -// license that can be found in the AdvantageKit-License.md file -// at the root directory of this project. +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. // // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of @@ -99,6 +105,7 @@ public class RobotContainer { // These are "Virtual Subsystems" that report information but have no motors private final Imu m_imu; + private final Vision m_vision; @SuppressWarnings("unused") private final Accelerometer m_accel; @@ -106,9 +113,6 @@ public class RobotContainer { @SuppressWarnings("unused") private final RBSIPowerMonitor m_power; - @SuppressWarnings("unused") - private final Vision m_vision; - @SuppressWarnings("unused") private List canHealth; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java index e58a4b3..ff4402c 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSimPhysics.java @@ -45,6 +45,7 @@ public class DriveSimPhysics { private final SwerveDriveKinematics kinematics; + /** Constructor */ public DriveSimPhysics( SwerveDriveKinematics kinematics, double moiKgMetersSq, double maxTorqueNm) { this.kinematics = kinematics; @@ -97,7 +98,7 @@ public void update(SwerveModuleState[] moduleStates, double dtSeconds) { pose = new Pose2d(newTranslation, newYaw); } - /* ================== Getters ================== */ + /** Getter Functions ***************************************************** */ public Pose2d getPose() { return pose; } @@ -114,7 +115,7 @@ public Translation2d getLinearAccel() { return linearAccel; } - /* ================== Reset ================== */ + /** Reset Functions ****************************************************** */ public void reset(Pose2d pose) { this.pose = pose; this.omegaRadPerSec = 0.0; diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index abc40ee..b57653d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -18,16 +18,26 @@ import frc.robot.Constants.DrivebaseConstants; import org.littletonrobotics.junction.Logger; +/** + * Module API Class + * + *

The Module class is the API-level class for a single swerve module, of which there are four on + * the robot. This is not a true subsystem, but an abstraction layer. + */ public class Module { + + // Define IO private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; + // Declare alerts here, and only set/unset during the periodic() loop. private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; private final Alert turnEncoderDisconnectedAlert; private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + /** Constructor */ public Module(ModuleIO io, int index) { this.io = io; this.index = index; @@ -44,6 +54,8 @@ public Module(ModuleIO io, int index) { AlertType.kError); } + /************************************************************************* */ + /** Periodic function that is called each robot cycle by the Drive class */ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); @@ -64,7 +76,19 @@ public void periodic() { turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); } - /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ + /** Forwards the simulation periodic call to the IO layer */ + public void simulationPeriodic() { + io.simulationPeriodic(); + } + + /** Module Action Functions ********************************************** */ + /** + * Runs the module with the specified setpoint state + * + *

Mutates the state to optimize it + * + * @param state The requested Swerve Modeule State + */ public void runSetpoint(SwerveModuleState state) { // Optimize velocity setpoint state.optimize(getAngle()); @@ -87,12 +111,17 @@ public void stop() { io.setTurnOpenLoop(0.0); } - /** Sets whether brake mode is enabled. */ + /** + * Sets whether brake mode is enabled + * + * @param enabled Is the brake enabled? + */ public void setBrakeMode(boolean enabled) { io.setDriveBrakeMode(enabled); io.setTurnBrakeMode(enabled); } + /** Getter functions ***************************************************** */ /** Returns the current turn angle of the module. */ public Rotation2d getAngle() { return inputs.turnPosition; @@ -128,11 +157,6 @@ public double[] getOdometryTimestamps() { return inputs.odometryTimestamps; } - /** Forwards the simulation periodic call to the IO layer */ - public void simulationPeriodic() { - io.simulationPeriodic(); - } - /** Returns the module position in radians. */ public double getWheelRadiusCharacterizationPosition() { return inputs.drivePositionRad; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 1005577..8e9c113 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -13,6 +13,7 @@ import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { + @AutoLog public static class ModuleIOInputs { public boolean driveConnected = false; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 6aec53e..ff9cd88 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -129,7 +129,7 @@ public class ModuleIOTalonFX implements ModuleIO { private long lastTimestampNano = System.nanoTime(); /* - * TalonFX I/O + * TalonFX I/O Constructor */ public ModuleIOTalonFX(int module) { // Record the module number for logging purposes @@ -260,6 +260,7 @@ public ModuleIOTalonFX(int module) { ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); } + /** Input Updating Loop ************************************************** */ @Override public void updateInputs(ModuleIOInputs inputs) { @@ -315,6 +316,7 @@ public void updateInputs(ModuleIOInputs inputs) { turnPositionQueue.clear(); } + /** Module Action Functions ********************************************** */ /** * Set the drive motor to an open-loop voltage, scaled to battery voltage * From 88ae89eb490a81aa195f5df1c785d00d68c17a34 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Tue, 3 Feb 2026 10:43:55 -0700 Subject: [PATCH 4/6] Clean non-ASCII characters --- src/main/java/frc/robot/subsystems/imu/ImuIOSim.java | 2 +- .../frc/robot/subsystems/vision/CameraSweepEvaluator.java | 6 +++--- .../robot/subsystems/vision/VisionIOPhotonVisionSim.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index 26820d4..a28212c 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -68,7 +68,7 @@ public void updateInputs(ImuIOInputs inputs) { inputs.yawRateRadPerSec = yawRateRadPerSec; inputs.linearAccel = new Translation3d(ax, ay, az); - // Jerk: SIM doesn’t have a prior accel here unless you want it; set to 0 by default. + // Jerk: SIM doesn't have a prior accel here unless you want it; set to 0 by default. // If you do want jerk, you can add prevAx/prevAy/prevAz + dt just like the real IO. inputs.linearJerk = Translation3d.kZero; diff --git a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java index 3cdcbb9..18f5679 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java @@ -82,7 +82,7 @@ private static Quaternion quatFromAxisAngle(double ax, double ay, double az, dou return new Quaternion(c, ax * s, ay * s, az * s); } - // Hamilton product: q = a ⊗ b + // Hamilton product: q = a x b (cross-product) private static Quaternion quatMul(Quaternion a, Quaternion b) { double aw = a.getW(), ax = a.getX(), ay = a.getY(), az = a.getZ(); double bw = b.getW(), bx = b.getX(), by = b.getY(), bz = b.getZ(); @@ -103,8 +103,8 @@ private static Quaternion quatMul(Quaternion a, Quaternion b) { * *

Conventions: yawDeg = rotation about +Z (up), pitchDeg = rotation about +Y. * - *

Order: extra = yaw ⊗ pitch (yaw first, then pitch, both in the camera/base frame) combined = - * base ⊗ extra + *

Order: extra = yaw x pitch (yaw first, then pitch, both in the camera/base frame) combined = + * base x extra */ private static Rotation3d composeCameraExtra(Rotation3d base, double yawDeg, double pitchDeg) { double yaw = Units.degreesToRadians(yawDeg); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 4e5761b..d97132d 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -62,7 +62,7 @@ public VisionIOPhotonVisionSim( @Override public void updateInputs(VisionIOInputs inputs) { // NOTE: This updates the sim world every time a sim camera is polled. - // That's fine (fast enough), but if you want “update once per loop,” see note below. + // That's fine (fast enough), but if you want "update once per loop,"" see note below. visionSim.update(poseSupplier.get()); // Then pull results like normal (and emit PoseObservation + usedTagIds sets) From a1c95390e2e9675f740c0aa8b24748cffd9bca2c Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Tue, 3 Feb 2026 17:44:20 -0700 Subject: [PATCH 5/6] Reduce drivebase periodic cycle time by using primitives --- src/main/java/frc/robot/Constants.java | 5 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/subsystems/drive/Drive.java | 98 ++++++++++++------- .../subsystems/drive/ModuleIOTalonFX.java | 70 ++++++++----- .../subsystems/drive/SwerveConstants.java | 5 +- .../frc/robot/subsystems/vision/Vision.java | 5 +- 6 files changed, 120 insertions(+), 67 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 16f185b..7d30eae 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,6 +45,7 @@ import frc.robot.util.RBSIEnum.SwerveType; import frc.robot.util.RBSIEnum.VisionType; import frc.robot.util.RobotDeviceId; +import java.util.Set; import org.photonvision.simulation.SimCameraProperties; import swervelib.math.Matter; @@ -418,8 +419,8 @@ public static final class AutoConstants { /** Vision Constants (Assuming PhotonVision) ***************************** */ public static class VisionConstants { - public static final java.util.Set kTrustedTags = - java.util.Set.of(2, 3, 4, 5, 8, 9, 10, 11, 18, 19, 20, 21, 24, 25, 26, 27); // HUB AprilTags + public static final Set kTrustedTags = + Set.of(2, 3, 4, 5, 8, 9, 10, 11, 18, 19, 20, 21, 24, 25, 26, 27); // HUB AprilTags // Noise scaling factors (lower = more trusted) public static final double kTrustedTagStdDevScale = 0.6; // 40% more weight diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc2643d..521e09d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -525,12 +525,12 @@ private void definesysIdRoutines() { private VisionIO[] buildVisionIOsReal(Drive drive) { return switch (Constants.getVisionType()) { case PHOTON -> - java.util.Arrays.stream(Constants.Cameras.ALL) + Arrays.stream(Constants.Cameras.ALL) .map(c -> (VisionIO) new VisionIOPhotonVision(c.name(), c.robotToCamera())) .toArray(VisionIO[]::new); case LIMELIGHT -> - java.util.Arrays.stream(Constants.Cameras.ALL) + Arrays.stream(Constants.Cameras.ALL) .map(c -> (VisionIO) new VisionIOLimelight(c.name(), drive::getHeading)) .toArray(VisionIO[]::new); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 0046db7..aeb71e6 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -277,17 +277,13 @@ public void rbsiPeriodic() { final double[] ts = modules[0].getOdometryTimestamps(); final int n = (ts == null) ? 0 : ts.length; - if (n == 0) { - // Still keep yaw buffers updated for yaw-gating callers - double now = Timer.getFPGATimestamp(); - yawBuffer.addSample(now, imuInputs.yawPositionRad); - yawRateBuffer.addSample(now, imuInputs.yawRateRadPerSec); - - gyroDisconnectedAlert.set(!imuInputs.connected); - return; + // Cache per-module histories ONCE (avoid repeated getters in the loop) + final SwerveModulePosition[][] modHist = new SwerveModulePosition[4][]; + for (int m = 0; m < 4; m++) { + modHist[m] = modules[m].getOdometryPositions(); } - // Optional IMU odometry yaw queue (preferred if aligned) + // Determine yaw queue availability final boolean hasYawQueue = imuInputs.connected && imuInputs.odometryYawTimestamps != null @@ -299,13 +295,59 @@ public void rbsiPeriodic() { final double[] yawTs = hasYawQueue ? imuInputs.odometryYawTimestamps : null; final double[] yawPos = hasYawQueue ? imuInputs.odometryYawPositionsRad : null; + // If we have no module samples, still keep yaw buffers “alive” for gating callers + if (n == 0) { + final double now = Timer.getFPGATimestamp(); + yawBuffer.addSample(now, imuInputs.yawPositionRad); + yawRateBuffer.addSample(now, imuInputs.yawRateRadPerSec); + + gyroDisconnectedAlert.set(!imuInputs.connected); + return; + } + + // Decide whether yaw queue is index-aligned with module[0] timestamps. + // We only trust index alignment if BOTH: + // - yaw has at least n samples + // - yawTs[i] ~= ts[i] for i in range (tight epsilon) + boolean yawIndexAligned = false; + if (hasYawQueue && yawTs.length >= n) { + yawIndexAligned = true; + final double eps = 1e-3; // 1 ms + for (int i = 0; i < n; i++) { + if (Math.abs(yawTs[i] - ts[i]) > eps) { + yawIndexAligned = false; + break; + } + } + } + + // If yaw is NOT index-aligned but we have a yaw queue, build yaw/yawRate buffers ONCE. + if (hasYawQueue && !yawIndexAligned) { + for (int k = 0; k < yawTs.length; k++) { + yawBuffer.addSample(yawTs[k], yawPos[k]); + if (k > 0) { + final double dt = yawTs[k] - yawTs[k - 1]; + if (dt > 1e-6) { + yawRateBuffer.addSample(yawTs[k], (yawPos[k] - yawPos[k - 1]) / dt); + } + } + } + } + + // If NO yaw queue, add a single “now” sample once (don’t do this per replay sample) + if (!hasYawQueue) { + final double now = Timer.getFPGATimestamp(); + yawBuffer.addSample(now, imuInputs.yawPositionRad); + yawRateBuffer.addSample(now, imuInputs.yawRateRadPerSec); + } + // Replay each odometry sample for (int i = 0; i < n; i++) { - // Build module positions at this sample index - for (int m = 0; m < 4; m++) { - SwerveModulePosition[] hist = modules[m].getOdometryPositions(); + final double t = ts[i]; - // Defensive: if one module returned fewer samples, clamp to last available + // Build module positions at this sample index (clamp defensively) + for (int m = 0; m < 4; m++) { + final SwerveModulePosition[] hist = modHist[m]; if (hist == null || hist.length == 0) { odomPositions[m] = modules[m].getPosition(); } else if (i < hist.length) { @@ -316,48 +358,30 @@ public void rbsiPeriodic() { } // Determine yaw at this timestamp - final double t = ts[i]; double yawRad = imuInputs.yawPositionRad; // fallback if (hasYawQueue) { - // If yaw queue is index-aligned with module timestamps, use by index. - // Otherwise, fall back to interpolating from yawBuffer. - if (yawTs.length > i && Math.abs(yawTs[i] - t) < 1e-3) { + if (yawIndexAligned) { yawRad = yawPos[i]; - // Keep yaw/yawRate buffers updated in the same timebase for gating queries + // Keep yaw/yawRate buffers updated in odometry timebase (good for yaw-gate) yawBuffer.addSample(t, yawRad); - if (i > 0 && yawTs.length > (i - 1)) { - double dt = yawTs[i] - yawTs[i - 1]; + if (i > 0) { + final double dt = yawTs[i] - yawTs[i - 1]; if (dt > 1e-6) { yawRateBuffer.addSample(t, (yawPos[i] - yawPos[i - 1]) / dt); } } } else { - // Not index-aligned; build yaw buffer from queue once, then sample it - // (cheap enough; queue is short) - for (int k = 0; k < yawTs.length; k++) { - yawBuffer.addSample(yawTs[k], yawPos[k]); - if (k > 0) { - double dt = yawTs[k] - yawTs[k - 1]; - if (dt > 1e-6) { - yawRateBuffer.addSample(yawTs[k], (yawPos[k] - yawPos[k - 1]) / dt); - } - } - } + // yawBuffer was pre-filled above; interpolate here yawRad = yawBuffer.getSample(t).orElse(imuInputs.yawPositionRad); } - } else { - // No yaw queue: store "now" primitives for gating users (still useful) - double now = Timer.getFPGATimestamp(); - yawBuffer.addSample(now, imuInputs.yawPositionRad); - yawRateBuffer.addSample(now, imuInputs.yawRateRadPerSec); } // Feed estimator at this historical timestamp m_PoseEstimator.updateWithTime(t, Rotation2d.fromRadians(yawRad), odomPositions); - // Maintain pose history in the SAME timebase as estimator + // Maintain pose history in SAME timebase as estimator poseBuffer.addSample(t, m_PoseEstimator.getEstimatedPosition()); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index ff9cd88..65da2c7 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -48,6 +48,7 @@ import frc.robot.generated.TunerConstants; import frc.robot.util.PhoenixUtil; import frc.robot.util.RBSICANBusRegistry; +import java.util.Arrays; import java.util.Queue; import org.littletonrobotics.junction.Logger; @@ -264,14 +265,14 @@ public ModuleIOTalonFX(int module) { @Override public void updateInputs(ModuleIOInputs inputs) { - // Refresh most signals + // -------------------- Refresh Phoenix signals -------------------- var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); - // Log *which* groups are failing and what the code is + // Log refresh failures explicitly (debug gold) if (!driveStatus.isOK()) { Logger.recordOutput("CAN/Module" + module + "/DriveRefreshStatus", driveStatus.toString()); } @@ -282,38 +283,63 @@ public void updateInputs(ModuleIOInputs inputs) { Logger.recordOutput("CAN/Module" + module + "/EncRefreshStatus", encStatus.toString()); } - // Update drive inputs + // -------------------- Connectivity flags -------------------- inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); + inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus.isOK()); + + // -------------------- Instantaneous state -------------------- inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); - // Update turn inputs - inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); - inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus.isOK()); inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); - // Update odometry inputs - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value)) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value)) - .toArray(Rotation2d[]::new); - - // Clear the queues - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); + // -------------------- Odometry queue drain -------------------- + final int tsCount = timestampQueue.size(); + final int driveCount = drivePositionQueue.size(); + final int turnCount = turnPositionQueue.size(); + + // Only consume the common prefix — guarantees alignment + final int sampleCount = Math.min(tsCount, Math.min(driveCount, turnCount)); + + if (sampleCount <= 0) { + inputs.odometryTimestamps = new double[0]; + inputs.odometryDrivePositionsRad = new double[0]; + inputs.odometryTurnPositions = new Rotation2d[0]; + return; + } + + final double[] outTs = new double[sampleCount]; + final double[] outDriveRad = new double[sampleCount]; + final Rotation2d[] outTurn = new Rotation2d[sampleCount]; + + for (int i = 0; i < sampleCount; i++) { + final Double t = timestampQueue.poll(); + final Double driveRot = drivePositionQueue.poll(); + final Double turnRot = turnPositionQueue.poll(); + + // Defensive guard (should never trigger, but keeps us safe) + if (t == null || driveRot == null || turnRot == null) { + inputs.odometryTimestamps = Arrays.copyOf(outTs, i); + inputs.odometryDrivePositionsRad = Arrays.copyOf(outDriveRad, i); + inputs.odometryTurnPositions = Arrays.copyOf(outTurn, i); + return; + } + + outTs[i] = t.doubleValue(); + outDriveRad[i] = Units.rotationsToRadians(driveRot.doubleValue()); + outTurn[i] = Rotation2d.fromRotations(turnRot.doubleValue()); + } + + inputs.odometryTimestamps = outTs; + inputs.odometryDrivePositionsRad = outDriveRad; + inputs.odometryTurnPositions = outTurn; } /** Module Action Functions ********************************************** */ diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index a9332ce..ceaff2b 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -22,6 +22,7 @@ import frc.robot.subsystems.imu.ImuIONavX; import frc.robot.subsystems.imu.ImuIOPigeon2; import frc.robot.util.YagslConstants; +import java.util.function.Supplier; /** * Holds the proper set of drive constants given the type of drive @@ -310,9 +311,9 @@ public enum ImuType { NAVX(new String[] {"navx", "navx_spi"}, ImuIONavX::new); private final String[] keys; - public final java.util.function.Supplier factory; + public final Supplier factory; - ImuType(String[] keys, java.util.function.Supplier factory) { + ImuType(String[] keys, Supplier factory) { this.keys = keys; this.factory = factory; } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 908cf9c..47f0bdb 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -34,6 +34,7 @@ import frc.robot.util.VirtualSubsystem; import java.util.ArrayDeque; import java.util.ArrayList; +import java.util.Arrays; import java.util.Optional; import java.util.OptionalDouble; import java.util.Set; @@ -93,7 +94,7 @@ public Vision(Drive drive, VisionConsumer consumer, VisionIO... io) { } this.lastAcceptedTsPerCam = new double[io.length]; - java.util.Arrays.fill(lastAcceptedTsPerCam, Double.NEGATIVE_INFINITY); + Arrays.fill(lastAcceptedTsPerCam, Double.NEGATIVE_INFINITY); // Log robot->camera transforms if available int n = Math.min(camConfigs.length, io.length); @@ -108,7 +109,7 @@ public Vision(Drive drive, VisionConsumer consumer, VisionIO... io) { public void resetPoseGate(double fpgaNowSeconds) { lastPoseResetTimestamp = fpgaNowSeconds; fusedBuffer.clear(); - java.util.Arrays.fill(lastAcceptedTsPerCam, Double.NEGATIVE_INFINITY); + Arrays.fill(lastAcceptedTsPerCam, Double.NEGATIVE_INFINITY); } /** Swap trusted tag set per event/field without redeploy. */ From 04aa59d818894969f7c616022ae26f19966e83a1 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Tue, 3 Feb 2026 17:59:25 -0700 Subject: [PATCH 6/6] Clean up the Vision module --- .../frc/robot/subsystems/vision/Vision.java | 118 +++++++++++------- .../frc/robot/subsystems/vision/VisionIO.java | 2 +- .../vision/VisionIOPhotonVisionSim.java | 2 +- 3 files changed, 72 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 47f0bdb..74d318a 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -43,6 +43,7 @@ public class Vision extends VirtualSubsystem { + /** Vision Consumer definition */ @FunctionalInterface public interface VisionConsumer { void accept(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix stdDevs); @@ -59,18 +60,16 @@ private record TimedEstimate(Pose2d pose, double ts, Matrix stdDevs) {} private final Constants.Cameras.CameraConfig[] camConfigs = Constants.Cameras.ALL; - // --- Per-camera monotonic gate --- + // Per-camera monotonic and pose reset gates private final double[] lastAcceptedTsPerCam; - - // --- Pose reset gate --- private volatile double lastPoseResetTimestamp = Double.NEGATIVE_INFINITY; - // --- Smoothing buffer (recent fused estimates) --- + // Smoothing buffer (recent fused estimates) private final ArrayDeque fusedBuffer = new ArrayDeque<>(); private final double smoothWindowSec = 0.25; private final int smoothMaxSize = 12; - // --- Trusted tags configuration (swappable per event/field) --- + // Trusted tags configuration (swappable per event/field) private final AtomicReference> trustedTags = new AtomicReference<>(Set.of()); private volatile boolean requireTrustedTag = false; @@ -78,11 +77,12 @@ private record TimedEstimate(Pose2d pose, double ts, Matrix stdDevs) {} private volatile double trustedTagStdDevScale = 0.70; // < 1 => more trusted private volatile double untrustedTagStdDevScale = 1.40; // > 1 => less trusted - // --- Optional 254-style yaw gate for single-tag --- + // Yaw-rate gate for single-tag measurements private volatile boolean enableSingleTagYawGate = true; private volatile double yawGateLookbackSec = 0.30; private volatile double yawGateLimitRadPerSec = 5.0; + /** Constructor */ public Vision(Drive drive, VisionConsumer consumer, VisionIO... io) { this.drive = drive; this.consumer = consumer; @@ -103,37 +103,7 @@ public Vision(Drive drive, VisionConsumer consumer, VisionIO... io) { } } - // -------- Runtime configuration hooks -------- - - /** Call when you reset odometry (e.g. auto start, manual reset, etc). */ - public void resetPoseGate(double fpgaNowSeconds) { - lastPoseResetTimestamp = fpgaNowSeconds; - fusedBuffer.clear(); - Arrays.fill(lastAcceptedTsPerCam, Double.NEGATIVE_INFINITY); - } - - /** Swap trusted tag set per event/field without redeploy. */ - public void setTrustedTags(Set tags) { - trustedTags.set(Set.copyOf(tags)); - } - - public void setRequireTrustedTag(boolean require) { - requireTrustedTag = require; - } - - public void setTrustedTagStdDevScales(double trustedScale, double untrustedScale) { - trustedTagStdDevScale = trustedScale; - untrustedTagStdDevScale = untrustedScale; - } - - public void setSingleTagYawGate(boolean enabled, double lookbackSec, double limitRadPerSec) { - enableSingleTagYawGate = enabled; - yawGateLookbackSec = lookbackSec; - yawGateLimitRadPerSec = limitRadPerSec; - } - - // -------- Core periodic -------- - + /** Periodic Function */ @Override public void rbsiPeriodic() { @@ -146,13 +116,13 @@ public void rbsiPeriodic() { Logger.recordOutput("Vision/PoseGateResetFromDrive", false); } - // 1) Update/log camera inputs + // Update/log camera inputs for (int i = 0; i < io.length; i++) { io[i].updateInputs(inputs[i]); Logger.processInputs("Vision/Camera" + i, inputs[i]); } - // 2) Pick one best accepted estimate per camera for this loop + // Pick one best accepted estimate per camera for this loop final ArrayList perCamAccepted = new ArrayList<>(io.length); for (int cam = 0; cam < io.length; cam++) { @@ -209,20 +179,20 @@ public void rbsiPeriodic() { if (perCamAccepted.isEmpty()) return; - // 3) Fusion time is the newest timestamp among accepted per-camera samples + // Fusion time is the newest timestamp among accepted per-camera samples double tF = perCamAccepted.stream().mapToDouble(e -> e.ts).max().orElse(Double.NaN); if (!Double.isFinite(tF)) return; - // 4) Time-align camera estimates to tF using odometry buffer, then inverse-variance fuse + // Time-align camera estimates to tF using odometry buffer, then inverse-variance fuse TimedEstimate fused = fuseAtTime(perCamAccepted, tF); if (fused == null) return; - // 5) Smooth by fusing recent fused estimates (also aligned to tF) + // Smooth by fusing recent fused estimates (also aligned to tF) pushFused(fused); TimedEstimate smoothed = smoothAtTime(tF); if (smoothed == null) return; - // 6) Inject + // Inject the pose consumer.accept(smoothed.pose, smoothed.ts, smoothed.stdDevs); Logger.recordOutput("Vision/FusedPose", fused.pose); @@ -230,8 +200,62 @@ public void rbsiPeriodic() { Logger.recordOutput("Vision/FusedTimestamp", tF); } - // -------- Gating + scoring -------- + /** Runtime configuration hooks ****************************************** */ + /** + * Call when you reset odometry (e.g. auto start, manual reset, etc). + * + * @param fpgaNowSeconds Timestamp for the pose gate reset + */ + public void resetPoseGate(double fpgaNowSeconds) { + lastPoseResetTimestamp = fpgaNowSeconds; + fusedBuffer.clear(); + Arrays.fill(lastAcceptedTsPerCam, Double.NEGATIVE_INFINITY); + } + + /** + * Swap trusted tag set per event/field without redeploy + * + * @param tags Set of trusted tags to use + */ + public void setTrustedTags(Set tags) { + trustedTags.set(Set.copyOf(tags)); + } + + /** + * Set whether to requite trusted tags + * + * @param require Boolean + */ + public void setRequireTrustedTag(boolean require) { + requireTrustedTag = require; + } + + /** + * Set the (un)trusted standard deviation scales + * + * @param trustedScale The scale for trusted tags + * @param untrustedScale The scale for untrusted tags + */ + public void setTrustedTagStdDevScales(double trustedScale, double untrustedScale) { + trustedTagStdDevScale = trustedScale; + untrustedTagStdDevScale = untrustedScale; + } + + /** + * Set the yaw gate for single-tag measurements + * + * @param enabled Enable the gate? + * @param lookbackSec Lookback time + * @param limitRadPerSec Yaw rate above which single-tag measurements will be ignored + */ + public void setSingleTagYawGate(boolean enabled, double lookbackSec, double limitRadPerSec) { + enableSingleTagYawGate = enabled; + yawGateLookbackSec = lookbackSec; + yawGateLimitRadPerSec = limitRadPerSec; + } + + /** Gating + Scoring ***************************************************** */ private static final class GateResult { final boolean accepted; @@ -344,8 +368,7 @@ private boolean isBetter(TimedEstimate a, TimedEstimate b) { return ta < tb; } - // -------- Time alignment + fusion -------- - + /** Time alignment & fusion ********************************************** */ private TimedEstimate fuseAtTime(ArrayList estimates, double tF) { final ArrayList aligned = new ArrayList<>(estimates.size()); for (var e : estimates) { @@ -411,8 +434,7 @@ private TimedEstimate inverseVarianceFuse(ArrayList alignedAtTF, return new TimedEstimate(fused, tF, fusedStd); } - // -------- Smoothing buffer -------- - + /** Smoothing buffer ***************************************************** */ private void pushFused(TimedEstimate fused) { fusedBuffer.addLast(fused); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index fad294d..7bc61ed 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -19,7 +19,7 @@ public interface VisionIO { class VisionIOInputs { public boolean connected = false; - /** Latest "servo to target" observation (optional) */ + /** Latest "camera to target" observation (optional) */ public TargetObservation latestTargetObservation = new TargetObservation(Rotation2d.kZero, Rotation2d.kZero); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index d97132d..f554cc4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -62,7 +62,7 @@ public VisionIOPhotonVisionSim( @Override public void updateInputs(VisionIOInputs inputs) { // NOTE: This updates the sim world every time a sim camera is polled. - // That's fine (fast enough), but if you want "update once per loop,"" see note below. + // That's fine (fast enough), but if you want "update once per loop," see note below. visionSim.update(poseSupplier.get()); // Then pull results like normal (and emit PoseObservation + usedTagIds sets)