rotationSupplier) {
+ SlewRateLimiter xLimiter = joystickLimiter();
+ SlewRateLimiter yLimiter = joystickLimiter();
// Construct command
return Commands.run(
() -> {
// Get linear velocity
Translation2d linearVelocity =
- getLinearVelocity(xSupplier.getAsDouble(), ySupplier.getAsDouble());
+ getLinearVelocity(
+ xLimiter.calculate(xSupplier.getAsDouble()),
+ yLimiter.calculate(ySupplier.getAsDouble()));
// Calculate angular speed
double omega =
@@ -133,52 +136,95 @@ public static Command fieldRelativeDriveAtAngle(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega);
- boolean isFlipped =
- DriverStation.getAlliance().isPresent()
- && DriverStation.getAlliance().get() == Alliance.Red;
drive.runVelocity(
- ChassisSpeeds.fromFieldRelativeSpeeds(
- speeds,
- isFlipped
- ? drive.getHeading().plus(new Rotation2d(Math.PI))
- : drive.getHeading()));
+ ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getAllianceAwareHeading(drive)));
},
drive)
// Reset PID controller when command starts & ends;
- .beforeStarting(() -> drive.resetHeadingController())
+ .beforeStarting(
+ () -> {
+ resetLimiters(xLimiter, yLimiter);
+ drive.resetHeadingController();
+ })
.finallyDo(() -> drive.resetHeadingController());
}
+ /** Holds the drive stopped while scheduled. */
+ public static Command stop(Drive drive) {
+ return Commands.run(drive::stop, drive).finallyDo(drive::stop);
+ }
+
+ /** Holds the modules in an X pattern while scheduled. */
+ public static Command stopWithX(Drive drive) {
+ return Commands.run(drive::stopWithX, drive).finallyDo(drive::stop);
+ }
+
+ /** Sets drive motor neutral mode. */
+ public static Command setBrakeMode(Drive drive, boolean brake) {
+ return Commands.runOnce(() -> drive.setMotorBrake(brake), drive).ignoringDisable(true);
+ }
+
+ /** Zeros field-relative heading using alliance-aware orientation. */
+ public static Command zeroHeadingForAlliance(Drive drive) {
+ return Commands.runOnce(drive::zeroHeadingForAlliance, drive).ignoringDisable(true);
+ }
+
+ /** Drives robot-relative at a fixed velocity while scheduled. */
+ public static Command robotRelativeNudge(
+ Drive drive, double vxMetersPerSec, double vyMetersPerSec, double omegaRadPerSec) {
+ ChassisSpeeds speeds = new ChassisSpeeds(vxMetersPerSec, vyMetersPerSec, omegaRadPerSec);
+ return Commands.run(() -> drive.runVelocity(speeds), drive).finallyDo(drive::stop);
+ }
+
/** Utility functions needed by commands in this module ****************** */
/**
* Compute the new linear velocity from inputs, including applying deadbands and squaring for
- * smoothness. Also apply the linear velocity Slew Rate Limiter.
+ * smoothness. Slew limiting is applied by the command factory before calling this helper.
*/
- private static Translation2d getLinearVelocity(double x, double y) {
+ static Translation2d getLinearVelocity(double x, double y) {
// Apply deadband
- double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), OperatorConstants.kDeadband);
- Rotation2d linearDirection = new Rotation2d(Math.atan2(y, x));
+ double linearMagnitude =
+ MathUtil.applyDeadband(Math.hypot(x, y), OperatorConstants.kJoystickDeadband);
+ if (linearMagnitude <= 0.0) {
+ return Translation2d.kZero;
+ }
// Square magnitude for more precise control
// NOTE: The x & y values range from -1 to +1, so their squares are as well
- linearMagnitude = linearMagnitude * linearMagnitude;
+ double scaledMagnitude = linearMagnitude * linearMagnitude;
+ double inputMagnitude = Math.hypot(x, y);
+ double scale = inputMagnitude > 1e-9 ? scaledMagnitude / inputMagnitude : 0.0;
// Return new linear velocity
- return new Pose2d(Translation2d.kZero, linearDirection)
- .transformBy(new Transform2d(linearMagnitude, 0.0, Rotation2d.kZero))
- .getTranslation();
+ return new Translation2d(x * scale, y * scale);
}
/**
* Compute the new angular velocity from inputs, including applying deadbands and squaring for
- * smoothness. Also apply the angular Slew Rate Limiter.
+ * smoothness. Slew limiting is applied by the command factory before calling this helper.
*/
- private static double getOmega(double omega) {
- omega = MathUtil.applyDeadband(omega, OperatorConstants.kDeadband);
+ static double getOmega(double omega) {
+ omega = MathUtil.applyDeadband(omega, OperatorConstants.kJoystickDeadband);
return Math.copySign(omega * omega, omega);
}
+ private static SlewRateLimiter joystickLimiter() {
+ return new SlewRateLimiter(OperatorConstants.kJoystickSlewRateLimit);
+ }
+
+ private static void resetLimiters(SlewRateLimiter... limiters) {
+ for (SlewRateLimiter limiter : limiters) {
+ limiter.reset(0.0);
+ }
+ }
+
+ private static Rotation2d getAllianceAwareHeading(Drive drive) {
+ return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red
+ ? drive.getHeading().plus(Rotation2d.k180deg)
+ : drive.getHeading();
+ }
+
/***************************************************************************/
/** DRIVEBASE CHARACTERIZATION COMMANDS ********************************** */
/**
@@ -187,131 +233,158 @@ private static double getOmega(double omega) {
* This command should only be used in voltage control mode.
*/
public static Command feedforwardCharacterization(Drive drive) {
- List velocitySamples = new LinkedList<>();
- List voltageSamples = new LinkedList<>();
+ List velocitySamples = new ArrayList<>();
+ List voltageSamples = new ArrayList<>();
Timer timer = new Timer();
return Commands.sequence(
- // Reset data
- Commands.runOnce(
- () -> {
- velocitySamples.clear();
- voltageSamples.clear();
- }),
-
- // Allow modules to orient
- Commands.run(
- () -> {
- drive.runCharacterization(0.0);
- },
- drive)
- .withTimeout(FF_START_DELAY),
-
- // Start timer
- Commands.runOnce(timer::restart),
-
- // Accelerate and gather data
- Commands.run(
- () -> {
- double voltage = timer.get() * FF_RAMP_RATE;
- drive.runCharacterization(voltage);
- velocitySamples.add(drive.getFFCharacterizationVelocity());
- voltageSamples.add(voltage);
- },
- drive)
-
- // When cancelled, calculate and print results
- .finallyDo(
- () -> {
- int n = velocitySamples.size();
- double sumX = 0.0;
- double sumY = 0.0;
- double sumXY = 0.0;
- double sumX2 = 0.0;
- for (int i = 0; i < n; i++) {
- sumX += velocitySamples.get(i);
- sumY += voltageSamples.get(i);
- sumXY += velocitySamples.get(i) * voltageSamples.get(i);
- sumX2 += velocitySamples.get(i) * velocitySamples.get(i);
- }
- double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX);
- double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX);
-
- NumberFormat formatter = new DecimalFormat("#0.00000");
- System.out.println("********** Drive FF Characterization Results **********");
- System.out.println("\tkS: " + formatter.format(kS));
- System.out.println("\tkV: " + formatter.format(kV));
- }));
- }
-
- /** Measures the robot's wheel radius by spinning in a circle. */
- public static Command wheelRadiusCharacterization(Drive drive) {
- SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE);
- WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState();
-
- return Commands.parallel(
- // Drive control sequence
- Commands.sequence(
- // Reset acceleration limiter
+ // Reset data
Commands.runOnce(
() -> {
- limiter.reset(0.0);
+ velocitySamples.clear();
+ voltageSamples.clear();
}),
- // Turn in place, accelerating up to full speed
+ // Allow modules to orient
Commands.run(
- () -> {
- double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY);
- drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed));
- },
- drive)),
-
- // Measurement sequence
- Commands.sequence(
- // Wait for modules to fully orient before starting measurement
- Commands.waitSeconds(1.0),
+ () -> {
+ drive.runCharacterization(0.0);
+ },
+ drive)
+ .withTimeout(DrivebaseConstants.kFeedforwardCharacterizationStartDelaySecs),
- // Record starting measurement
- Commands.runOnce(
- () -> {
- state.positions = drive.getWheelRadiusCharacterizationPositions();
- state.lastAngle = drive.getHeading();
- state.gyroDelta = 0.0;
- }),
+ // Start timer
+ Commands.runOnce(timer::restart),
- // Update gyro delta
+ // Accelerate and gather data
Commands.run(
() -> {
- var rotation = drive.getHeading();
- state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians());
- state.lastAngle = rotation;
- })
+ double voltage =
+ timer.get()
+ * DrivebaseConstants.kFeedforwardCharacterizationRampRateVoltsPerSec;
+ drive.runCharacterization(voltage);
+ velocitySamples.add(drive.getFFCharacterizationVelocity());
+ voltageSamples.add(voltage);
+ },
+ drive)
// When cancelled, calculate and print results
.finallyDo(
() -> {
- double[] positions = drive.getWheelRadiusCharacterizationPositions();
- double wheelDelta = 0.0;
- for (int i = 0; i < 4; i++) {
- wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0;
+ int n = velocitySamples.size();
+ double sumX = 0.0;
+ double sumY = 0.0;
+ double sumXY = 0.0;
+ double sumX2 = 0.0;
+ for (int i = 0; i < n; i++) {
+ sumX += velocitySamples.get(i);
+ sumY += voltageSamples.get(i);
+ sumXY += velocitySamples.get(i) * voltageSamples.get(i);
+ sumX2 += velocitySamples.get(i) * velocitySamples.get(i);
+ }
+ double denominator = n * sumX2 - sumX * sumX;
+ if (n < 2 || Math.abs(denominator) < 1e-9) {
+ DriverStation.reportWarning(
+ "Drive FF characterization did not collect enough usable samples.",
+ false);
+ drive.stop();
+ return;
}
- double wheelRadius =
- (state.gyroDelta * SwerveConstants.kDriveBaseRadiusMeters) / wheelDelta;
-
- NumberFormat formatter = new DecimalFormat("#0.000");
- System.out.println(
- "********** Wheel Radius Characterization Results **********");
- System.out.println(
- "\tWheel Delta: " + formatter.format(wheelDelta) + " radians");
- System.out.println(
- "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians");
- System.out.println(
- "\tWheel Radius: "
- + formatter.format(wheelRadius)
- + " meters, "
- + formatter.format(Units.metersToInches(wheelRadius))
- + " inches");
- })));
+
+ double kS = (sumY * sumX2 - sumX * sumXY) / denominator;
+ double kV = (n * sumXY - sumX * sumY) / denominator;
+
+ NumberFormat formatter = new DecimalFormat("#0.00000");
+ System.out.println("********** Drive FF Characterization Results **********");
+ System.out.println("\tkS: " + formatter.format(kS));
+ System.out.println("\tkV: " + formatter.format(kV));
+ drive.stop();
+ }))
+ .finallyDo(drive::stop);
+ }
+
+ /** Measures the robot's wheel radius by spinning in a circle. */
+ public static Command wheelRadiusCharacterization(Drive drive) {
+ SlewRateLimiter limiter =
+ new SlewRateLimiter(DrivebaseConstants.kWheelRadiusCharacterizationRampRateRadPerSecSq);
+ WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState();
+
+ return Commands.parallel(
+ // Drive control sequence
+ Commands.sequence(
+ // Reset acceleration limiter
+ Commands.runOnce(
+ () -> {
+ limiter.reset(0.0);
+ }),
+
+ // Turn in place, accelerating up to full speed
+ Commands.run(
+ () -> {
+ double speed =
+ limiter.calculate(
+ DrivebaseConstants.kWheelRadiusCharacterizationMaxVelocityRadPerSec);
+ drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed));
+ },
+ drive)),
+
+ // Measurement sequence
+ Commands.sequence(
+ // Wait for modules to fully orient before starting measurement
+ Commands.waitSeconds(DrivebaseConstants.kWheelRadiusCharacterizationStartDelaySecs),
+
+ // Record starting measurement
+ Commands.runOnce(
+ () -> {
+ state.positions = drive.getWheelRadiusCharacterizationPositions();
+ state.lastAngle = drive.getHeading();
+ state.gyroDelta = 0.0;
+ }),
+
+ // Update gyro delta
+ Commands.run(
+ () -> {
+ var rotation = drive.getHeading();
+ state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians());
+ state.lastAngle = rotation;
+ })
+
+ // When cancelled, calculate and print results
+ .finallyDo(
+ () -> {
+ double[] positions = drive.getWheelRadiusCharacterizationPositions();
+ double wheelDelta = 0.0;
+ for (int i = 0; i < 4; i++) {
+ wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0;
+ }
+ if (wheelDelta <= 1e-9) {
+ DriverStation.reportWarning(
+ "Wheel radius characterization did not measure wheel movement.",
+ false);
+ drive.stop();
+ return;
+ }
+
+ double wheelRadius =
+ (state.gyroDelta * SwerveConstants.kDriveBaseRadiusMeters)
+ / wheelDelta;
+
+ NumberFormat formatter = new DecimalFormat("#0.000");
+ System.out.println(
+ "********** Wheel Radius Characterization Results **********");
+ System.out.println(
+ "\tWheel Delta: " + formatter.format(wheelDelta) + " radians");
+ System.out.println(
+ "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians");
+ System.out.println(
+ "\tWheel Radius: "
+ + formatter.format(wheelRadius)
+ + " meters, "
+ + formatter.format(Units.metersToInches(wheelRadius))
+ + " inches");
+ drive.stop();
+ })))
+ .finallyDo(drive::stop);
}
private static class WheelRadiusCharacterizationState {
diff --git a/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java b/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java
new file mode 100644
index 00000000..91b48bfe
--- /dev/null
+++ b/src/main/java/frc/robot/generated/COMPBOTTunerConstants.java
@@ -0,0 +1,315 @@
+package frc.robot.generated;
+
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.hardware.*;
+import com.ctre.phoenix6.signals.*;
+import com.ctre.phoenix6.swerve.*;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.*;
+
+// import frc.robot.subsystems.CommandSwerveDrivetrain;
+
+// Generated by the 2026 Tuner X Swerve Project Generator
+// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+public class COMPBOTTunerConstants {
+ // Both sets of gains need to be tuned to your individual robot.
+
+ // The steer motor uses any SwerveModule.SteerRequestType control request with the
+ // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+ private static final Slot0Configs steerGains =
+ new Slot0Configs()
+ .withKP(100)
+ .withKI(0)
+ .withKD(0.5)
+ .withKS(0.1)
+ .withKV(2.66)
+ .withKA(0)
+ .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+ // When using closed-loop control, the drive motor uses the control
+ // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+ private static final Slot0Configs driveGains =
+ new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
+
+ // The closed-loop output type to use for the steer motors;
+ // This affects the PID/FF gains for the steer motors
+ private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
+ // The closed-loop output type to use for the drive motors;
+ // This affects the PID/FF gains for the drive motors
+ private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
+
+ // The type of motor used for the drive motor
+ private static final DriveMotorArrangement kDriveMotorType =
+ DriveMotorArrangement.TalonFX_Integrated;
+ // The type of motor used for the drive motor
+ private static final SteerMotorArrangement kSteerMotorType =
+ SteerMotorArrangement.TalonFX_Integrated;
+
+ // The remote sensor feedback type to use for the steer motors;
+ // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
+ private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
+
+ // The stator current at which the wheels start to slip;
+ // This needs to be tuned to your individual robot
+ private static final Current kSlipCurrent = Amps.of(120.0);
+
+ // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
+ // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
+ private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+ private static final TalonFXConfiguration steerInitialConfigs =
+ new TalonFXConfiguration()
+ .withCurrentLimits(
+ new CurrentLimitsConfigs()
+ // Swerve azimuth does not require much torque output, so we can set a relatively
+ // low
+ // stator current limit to help avoid brownouts without impacting performance.
+ .withStatorCurrentLimit(Amps.of(60))
+ .withStatorCurrentLimitEnable(true));
+ private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
+ // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
+ private static final Pigeon2Configuration pigeonConfigs = null;
+
+ // CAN bus that the devices are located on;
+ // All swerve devices must share the same CAN bus
+ public static final CANBus kCANBus = new CANBus("DriveTrain", "./logs/example.hoot");
+
+ // Theoretical free speed (m/s) at 12 V applied output;
+ // This needs to be tuned to your individual robot
+ public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73);
+
+ // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+ // This may need to be tuned to your individual robot
+ private static final double kCoupleRatio = 3.5714285714285716;
+
+ private static final double kDriveGearRatio = 6.746031746031747;
+ private static final double kSteerGearRatio = 21.428571428571427;
+ private static final Distance kWheelRadius = Inches.of(2);
+
+ private static final boolean kInvertLeftSide = false;
+ private static final boolean kInvertRightSide = true;
+
+ private static final int kPigeonId = 13;
+
+ // These are only used for simulation
+ private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
+ private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
+ // Simulated voltage necessary to overcome friction
+ private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
+ private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
+
+ public static final SwerveDrivetrainConstants DrivetrainConstants =
+ new SwerveDrivetrainConstants()
+ .withCANBusName(kCANBus.getName())
+ .withPigeon2Id(kPigeonId)
+ .withPigeon2Configs(pigeonConfigs);
+
+ private static final SwerveModuleConstantsFactory<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ ConstantCreator =
+ new SwerveModuleConstantsFactory<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
+ .withDriveMotorGearRatio(kDriveGearRatio)
+ .withSteerMotorGearRatio(kSteerGearRatio)
+ .withCouplingGearRatio(kCoupleRatio)
+ .withWheelRadius(kWheelRadius)
+ .withSteerMotorGains(steerGains)
+ .withDriveMotorGains(driveGains)
+ .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
+ .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
+ .withSlipCurrent(kSlipCurrent)
+ .withSpeedAt12Volts(kSpeedAt12Volts)
+ .withDriveMotorType(kDriveMotorType)
+ .withSteerMotorType(kSteerMotorType)
+ .withFeedbackSource(kSteerFeedbackType)
+ .withDriveMotorInitialConfigs(driveInitialConfigs)
+ .withSteerMotorInitialConfigs(steerInitialConfigs)
+ .withEncoderInitialConfigs(encoderInitialConfigs)
+ .withSteerInertia(kSteerInertia)
+ .withDriveInertia(kDriveInertia)
+ .withSteerFrictionVoltage(kSteerFrictionVoltage)
+ .withDriveFrictionVoltage(kDriveFrictionVoltage);
+
+ // Front Left
+ private static final int kFrontLeftDriveMotorId = 1;
+ private static final int kFrontLeftSteerMotorId = 2;
+ private static final int kFrontLeftEncoderId = 3;
+ private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.0);
+ private static final boolean kFrontLeftSteerMotorInverted = true;
+ private static final boolean kFrontLeftEncoderInverted = false;
+
+ private static final Distance kFrontLeftXPos = Inches.of(10.0);
+ private static final Distance kFrontLeftYPos = Inches.of(10.0);
+
+ // Front Right
+ private static final int kFrontRightDriveMotorId = 4;
+ private static final int kFrontRightSteerMotorId = 5;
+ private static final int kFrontRightEncoderId = 6;
+ private static final Angle kFrontRightEncoderOffset = Rotations.of(0.0);
+ private static final boolean kFrontRightSteerMotorInverted = true;
+ private static final boolean kFrontRightEncoderInverted = false;
+
+ private static final Distance kFrontRightXPos = Inches.of(10.0);
+ private static final Distance kFrontRightYPos = Inches.of(-10.0);
+
+ // Back Left
+ private static final int kBackLeftDriveMotorId = 7;
+ private static final int kBackLeftSteerMotorId = 8;
+ private static final int kBackLeftEncoderId = 9;
+ private static final Angle kBackLeftEncoderOffset = Rotations.of(0.0);
+ private static final boolean kBackLeftSteerMotorInverted = true;
+ private static final boolean kBackLeftEncoderInverted = false;
+
+ private static final Distance kBackLeftXPos = Inches.of(-10.0);
+ private static final Distance kBackLeftYPos = Inches.of(10.0);
+
+ // Back Right
+ private static final int kBackRightDriveMotorId = 10;
+ private static final int kBackRightSteerMotorId = 11;
+ private static final int kBackRightEncoderId = 12;
+ private static final Angle kBackRightEncoderOffset = Rotations.of(0.0);
+ private static final boolean kBackRightSteerMotorInverted = true;
+ private static final boolean kBackRightEncoderInverted = false;
+
+ private static final Distance kBackRightXPos = Inches.of(-10.0);
+ private static final Distance kBackRightYPos = Inches.of(-10.0);
+
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ FrontLeft =
+ ConstantCreator.createModuleConstants(
+ kFrontLeftSteerMotorId,
+ kFrontLeftDriveMotorId,
+ kFrontLeftEncoderId,
+ kFrontLeftEncoderOffset,
+ kFrontLeftXPos,
+ kFrontLeftYPos,
+ kInvertLeftSide,
+ kFrontLeftSteerMotorInverted,
+ kFrontLeftEncoderInverted);
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ FrontRight =
+ ConstantCreator.createModuleConstants(
+ kFrontRightSteerMotorId,
+ kFrontRightDriveMotorId,
+ kFrontRightEncoderId,
+ kFrontRightEncoderOffset,
+ kFrontRightXPos,
+ kFrontRightYPos,
+ kInvertRightSide,
+ kFrontRightSteerMotorInverted,
+ kFrontRightEncoderInverted);
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ BackLeft =
+ ConstantCreator.createModuleConstants(
+ kBackLeftSteerMotorId,
+ kBackLeftDriveMotorId,
+ kBackLeftEncoderId,
+ kBackLeftEncoderOffset,
+ kBackLeftXPos,
+ kBackLeftYPos,
+ kInvertLeftSide,
+ kBackLeftSteerMotorInverted,
+ kBackLeftEncoderInverted);
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ BackRight =
+ ConstantCreator.createModuleConstants(
+ kBackRightSteerMotorId,
+ kBackRightDriveMotorId,
+ kBackRightEncoderId,
+ kBackRightEncoderOffset,
+ kBackRightXPos,
+ kBackRightYPos,
+ kInvertRightSide,
+ kBackRightSteerMotorInverted,
+ kBackRightEncoderInverted);
+
+ // /**
+ // * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot
+ // * program,.
+ // */
+ // public static CommandSwerveDrivetrain createDrivetrain() {
+ // return new CommandSwerveDrivetrain(
+ // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
+ // }
+
+ /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */
+ public static class TunerSwerveDrivetrain extends SwerveDrivetrain {
+ /**
+ * Constructs a CTRE SwerveDrivetrain using the specified constants.
+ *
+ * This constructs the underlying hardware devices, so users should not construct the devices
+ * themselves. If they need the devices, they can access them through getters in the classes.
+ *
+ * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+ * @param modules Constants for each specific module
+ */
+ public TunerSwerveDrivetrain(
+ SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) {
+ super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules);
+ }
+
+ /**
+ * Constructs a CTRE SwerveDrivetrain using the specified constants.
+ *
+ *
This constructs the underlying hardware devices, so users should not construct the devices
+ * themselves. If they need the devices, they can access them through getters in the classes.
+ *
+ * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+ * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set
+ * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
+ * @param modules Constants for each specific module
+ */
+ public TunerSwerveDrivetrain(
+ SwerveDrivetrainConstants drivetrainConstants,
+ double odometryUpdateFrequency,
+ SwerveModuleConstants... modules) {
+ super(
+ TalonFX::new,
+ TalonFX::new,
+ CANcoder::new,
+ drivetrainConstants,
+ odometryUpdateFrequency,
+ modules);
+ }
+
+ /**
+ * Constructs a CTRE SwerveDrivetrain using the specified constants.
+ *
+ *
This constructs the underlying hardware devices, so users should not construct the devices
+ * themselves. If they need the devices, they can access them through getters in the classes.
+ *
+ * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+ * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set
+ * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
+ * @param odometryStandardDeviation The standard deviation for odometry calculation in the form
+ * [x, y, theta]áµ€, with units in meters and radians
+ * @param visionStandardDeviation The standard deviation for vision calculation in the form [x,
+ * y, theta]áµ€, with units in meters and radians
+ * @param modules Constants for each specific module
+ */
+ public TunerSwerveDrivetrain(
+ SwerveDrivetrainConstants drivetrainConstants,
+ double odometryUpdateFrequency,
+ Matrix odometryStandardDeviation,
+ Matrix visionStandardDeviation,
+ SwerveModuleConstants... modules) {
+ super(
+ TalonFX::new,
+ TalonFX::new,
+ CANcoder::new,
+ drivetrainConstants,
+ odometryUpdateFrequency,
+ odometryStandardDeviation,
+ visionStandardDeviation,
+ modules);
+ }
+ }
+}
diff --git a/src/main/java/frc/robot/generated/COMPBOTTunerView.java b/src/main/java/frc/robot/generated/COMPBOTTunerView.java
new file mode 100644
index 00000000..480ab82a
--- /dev/null
+++ b/src/main/java/frc/robot/generated/COMPBOTTunerView.java
@@ -0,0 +1,43 @@
+package frc.robot.generated;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+
+public final class COMPBOTTunerView implements TunerView {
+ @Override
+ public CANBus canBus() {
+ return COMPBOTTunerConstants.kCANBus;
+ }
+
+ @Override
+ public SwerveDrivetrainConstants drivetrain() {
+ return COMPBOTTunerConstants.DrivetrainConstants;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ frontLeft() {
+ return COMPBOTTunerConstants.FrontLeft;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ frontRight() {
+ return COMPBOTTunerConstants.FrontRight;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ backLeft() {
+ return COMPBOTTunerConstants.BackLeft;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ backRight() {
+ return COMPBOTTunerConstants.BackRight;
+ }
+}
diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java
old mode 100644
new mode 100755
similarity index 93%
rename from src/main/java/frc/robot/generated/TunerConstants.java
rename to src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java
index 3e23fec1..0942cc39
--- a/src/main/java/frc/robot/generated/TunerConstants.java
+++ b/src/main/java/frc/robot/generated/DEVBOT1TunerConstants.java
@@ -15,9 +15,9 @@
// import frc.robot.subsystems.CommandSwerveDrivetrain;
-// Generated by the 2026 Tuner X Swerve Project Generator
+// Generated by the Tuner X Swerve Project Generator
// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
-public class TunerConstants {
+public class DEVBOT1TunerConstants {
// Both sets of gains need to be tuned to your individual robot.
// The steer motor uses any SwerveModule.SteerRequestType control request with the
@@ -51,7 +51,7 @@ public class TunerConstants {
SteerMotorArrangement.TalonFX_Integrated;
// The remote sensor feedback type to use for the steer motors;
- // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
+ // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
// The stator current at which the wheels start to slip;
@@ -80,15 +80,15 @@ public class TunerConstants {
// Theoretical free speed (m/s) at 12 V applied output;
// This needs to be tuned to your individual robot
- public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21);
+ public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73);
// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
// This may need to be tuned to your individual robot
private static final double kCoupleRatio = 3.5714285714285716;
- private static final double kDriveGearRatio = 6.122448979591837;
+ private static final double kDriveGearRatio = 6.746031746031747;
private static final double kSteerGearRatio = 21.428571428571427;
- private static final Distance kWheelRadius = Inches.of(2.0);
+ private static final Distance kWheelRadius = Inches.of(2);
private static final boolean kInvertLeftSide = false;
private static final boolean kInvertRightSide = true;
@@ -138,45 +138,45 @@ public class TunerConstants {
private static final int kFrontLeftDriveMotorId = 1;
private static final int kFrontLeftSteerMotorId = 2;
private static final int kFrontLeftEncoderId = 3;
- private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.143310546875);
+ private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.0);
private static final boolean kFrontLeftSteerMotorInverted = true;
private static final boolean kFrontLeftEncoderInverted = false;
- private static final Distance kFrontLeftXPos = Inches.of(11.375);
- private static final Distance kFrontLeftYPos = Inches.of(11.375);
+ private static final Distance kFrontLeftXPos = Inches.of(10.0);
+ private static final Distance kFrontLeftYPos = Inches.of(10.0);
// Front Right
private static final int kFrontRightDriveMotorId = 4;
private static final int kFrontRightSteerMotorId = 5;
private static final int kFrontRightEncoderId = 6;
- private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.328125);
+ private static final Angle kFrontRightEncoderOffset = Rotations.of(0.0);
private static final boolean kFrontRightSteerMotorInverted = true;
private static final boolean kFrontRightEncoderInverted = false;
- private static final Distance kFrontRightXPos = Inches.of(11.375);
- private static final Distance kFrontRightYPos = Inches.of(-11.375);
+ private static final Distance kFrontRightXPos = Inches.of(10.0);
+ private static final Distance kFrontRightYPos = Inches.of(-10.0);
// Back Left
private static final int kBackLeftDriveMotorId = 7;
private static final int kBackLeftSteerMotorId = 8;
private static final int kBackLeftEncoderId = 9;
- private static final Angle kBackLeftEncoderOffset = Rotations.of(0.135498046875);
+ private static final Angle kBackLeftEncoderOffset = Rotations.of(0.0);
private static final boolean kBackLeftSteerMotorInverted = true;
private static final boolean kBackLeftEncoderInverted = false;
- private static final Distance kBackLeftXPos = Inches.of(-11.375);
- private static final Distance kBackLeftYPos = Inches.of(11.375);
+ private static final Distance kBackLeftXPos = Inches.of(-10.0);
+ private static final Distance kBackLeftYPos = Inches.of(10.0);
// Back Right
private static final int kBackRightDriveMotorId = 10;
private static final int kBackRightSteerMotorId = 11;
private static final int kBackRightEncoderId = 12;
- private static final Angle kBackRightEncoderOffset = Rotations.of(0.478515625);
+ private static final Angle kBackRightEncoderOffset = Rotations.of(0.0);
private static final boolean kBackRightSteerMotorInverted = true;
private static final boolean kBackRightEncoderInverted = false;
- private static final Distance kBackRightXPos = Inches.of(-11.375);
- private static final Distance kBackRightYPos = Inches.of(-11.375);
+ private static final Distance kBackRightXPos = Inches.of(-10.0);
+ private static final Distance kBackRightYPos = Inches.of(-10.0);
public static final SwerveModuleConstants<
TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
diff --git a/src/main/java/frc/robot/generated/DEVBOT1TunerView.java b/src/main/java/frc/robot/generated/DEVBOT1TunerView.java
new file mode 100644
index 00000000..46d19bca
--- /dev/null
+++ b/src/main/java/frc/robot/generated/DEVBOT1TunerView.java
@@ -0,0 +1,43 @@
+package frc.robot.generated;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+
+public final class DEVBOT1TunerView implements TunerView {
+ @Override
+ public CANBus canBus() {
+ return DEVBOT1TunerConstants.kCANBus;
+ }
+
+ @Override
+ public SwerveDrivetrainConstants drivetrain() {
+ return DEVBOT1TunerConstants.DrivetrainConstants;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ frontLeft() {
+ return DEVBOT1TunerConstants.FrontLeft;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ frontRight() {
+ return DEVBOT1TunerConstants.FrontRight;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ backLeft() {
+ return DEVBOT1TunerConstants.BackLeft;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ backRight() {
+ return DEVBOT1TunerConstants.BackRight;
+ }
+}
diff --git a/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java b/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java
new file mode 100644
index 00000000..31b86369
--- /dev/null
+++ b/src/main/java/frc/robot/generated/DEVBOT2TunerConstants.java
@@ -0,0 +1,322 @@
+package frc.robot.generated;
+
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.signals.*;
+import com.ctre.phoenix6.swerve.*;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
+import edu.wpi.first.units.measure.*;
+
+// import frc.robot.subsystems.CommandSwerveDrivetrain;
+
+// Generated by the 2026 Tuner X Swerve Project Generator
+// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+public class DEVBOT2TunerConstants {
+ // Both sets of gains need to be tuned to your individual robot.
+
+ // The steer motor uses any SwerveModule.SteerRequestType control request with the
+ // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+ private static final Slot0Configs steerGains =
+ new Slot0Configs()
+ .withKP(100)
+ .withKI(0)
+ .withKD(0.5)
+ .withKS(0.1)
+ .withKV(2.66)
+ .withKA(0)
+ .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+ // When using closed-loop control, the drive motor uses the control
+ // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+ private static final Slot0Configs driveGains =
+ new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
+
+ // The closed-loop output type to use for the steer motors;
+ // This affects the PID/FF gains for the steer motors
+ private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
+ // The closed-loop output type to use for the drive motors;
+ // This affects the PID/FF gains for the drive motors
+ private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
+
+ // The type of motor used for the drive motor
+ private static final DriveMotorArrangement kDriveMotorType =
+ DriveMotorArrangement.TalonFX_Integrated;
+ // The type of motor used for the drive motor
+ private static final SteerMotorArrangement kSteerMotorType =
+ SteerMotorArrangement.TalonFX_Integrated;
+
+ // The remote sensor feedback type to use for the steer motors;
+ // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
+ private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
+
+ // The stator current at which the wheels start to slip;
+ // This needs to be tuned to your individual robot
+ private static final Current kSlipCurrent = Amps.of(120.0);
+
+ // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
+ // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
+ private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+ private static final TalonFXConfiguration steerInitialConfigs =
+ new TalonFXConfiguration()
+ .withCurrentLimits(
+ new CurrentLimitsConfigs()
+ // Swerve azimuth does not require much torque output, so we can set a relatively
+ // low
+ // stator current limit to help avoid brownouts without impacting performance.
+ .withStatorCurrentLimit(Amps.of(60))
+ .withStatorCurrentLimitEnable(true));
+ private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
+ // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
+ private static final Pigeon2Configuration pigeonConfigs = null;
+
+ // CAN bus that the devices are located on;
+ // All swerve devices must share the same CAN bus
+ public static final CANBus kCANBus = new CANBus("DriveTrain", "./logs/example.hoot");
+
+ // Theoretical free speed (m/s) at 12 V applied output;
+ // This needs to be tuned to your individual robot
+ public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73);
+
+ // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
+ // This may need to be tuned to your individual robot
+ private static final double kCoupleRatio = 3.5714285714285716;
+
+ private static final double kDriveGearRatio = 6.746031746031747;
+ private static final double kSteerGearRatio = 21.428571428571427;
+ private static final Distance kWheelRadius = Inches.of(2);
+
+ private static final boolean kInvertLeftSide = false;
+ private static final boolean kInvertRightSide = true;
+
+ private static final int kPigeonId = 13;
+
+ // These are only used for simulation
+ private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
+ private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
+ // Simulated voltage necessary to overcome friction
+ private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
+ private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
+
+ public static final SwerveDrivetrainConstants DrivetrainConstants =
+ new SwerveDrivetrainConstants()
+ .withCANBusName(kCANBus.getName())
+ .withPigeon2Id(kPigeonId)
+ .withPigeon2Configs(pigeonConfigs);
+
+ private static final SwerveModuleConstantsFactory<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ ConstantCreator =
+ new SwerveModuleConstantsFactory<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
+ .withDriveMotorGearRatio(kDriveGearRatio)
+ .withSteerMotorGearRatio(kSteerGearRatio)
+ .withCouplingGearRatio(kCoupleRatio)
+ .withWheelRadius(kWheelRadius)
+ .withSteerMotorGains(steerGains)
+ .withDriveMotorGains(driveGains)
+ .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
+ .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
+ .withSlipCurrent(kSlipCurrent)
+ .withSpeedAt12Volts(kSpeedAt12Volts)
+ .withDriveMotorType(kDriveMotorType)
+ .withSteerMotorType(kSteerMotorType)
+ .withFeedbackSource(kSteerFeedbackType)
+ .withDriveMotorInitialConfigs(driveInitialConfigs)
+ .withSteerMotorInitialConfigs(steerInitialConfigs)
+ .withEncoderInitialConfigs(encoderInitialConfigs)
+ .withSteerInertia(kSteerInertia)
+ .withDriveInertia(kDriveInertia)
+ .withSteerFrictionVoltage(kSteerFrictionVoltage)
+ .withDriveFrictionVoltage(kDriveFrictionVoltage);
+
+ // Front Left
+ private static final int kFrontLeftDriveMotorId = 1;
+ private static final int kFrontLeftSteerMotorId = 2;
+ private static final int kFrontLeftEncoderId = 3;
+ private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.0);
+ private static final boolean kFrontLeftSteerMotorInverted = true;
+ private static final boolean kFrontLeftEncoderInverted = false;
+
+ private static final Distance kFrontLeftXPos = Inches.of(10.0);
+ private static final Distance kFrontLeftYPos = Inches.of(10.0);
+
+ // Front Right
+ private static final int kFrontRightDriveMotorId = 4;
+ private static final int kFrontRightSteerMotorId = 5;
+ private static final int kFrontRightEncoderId = 6;
+ private static final Angle kFrontRightEncoderOffset = Rotations.of(0.0);
+ private static final boolean kFrontRightSteerMotorInverted = true;
+ private static final boolean kFrontRightEncoderInverted = false;
+
+ private static final Distance kFrontRightXPos = Inches.of(10.0);
+ private static final Distance kFrontRightYPos = Inches.of(-10.0);
+
+ // Back Left
+ private static final int kBackLeftDriveMotorId = 7;
+ private static final int kBackLeftSteerMotorId = 8;
+ private static final int kBackLeftEncoderId = 9;
+ private static final Angle kBackLeftEncoderOffset = Rotations.of(0.0);
+ private static final boolean kBackLeftSteerMotorInverted = true;
+ private static final boolean kBackLeftEncoderInverted = false;
+
+ private static final Distance kBackLeftXPos = Inches.of(-10.0);
+ private static final Distance kBackLeftYPos = Inches.of(10.0);
+
+ // Back Right
+ private static final int kBackRightDriveMotorId = 10;
+ private static final int kBackRightSteerMotorId = 11;
+ private static final int kBackRightEncoderId = 12;
+ private static final Angle kBackRightEncoderOffset = Rotations.of(0.0);
+ private static final boolean kBackRightSteerMotorInverted = true;
+ private static final boolean kBackRightEncoderInverted = false;
+
+ private static final Distance kBackRightXPos = Inches.of(-10.0);
+ private static final Distance kBackRightYPos = Inches.of(-10.0);
+
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ FrontLeft =
+ ConstantCreator.createModuleConstants(
+ kFrontLeftSteerMotorId,
+ kFrontLeftDriveMotorId,
+ kFrontLeftEncoderId,
+ kFrontLeftEncoderOffset,
+ kFrontLeftXPos,
+ kFrontLeftYPos,
+ kInvertLeftSide,
+ kFrontLeftSteerMotorInverted,
+ kFrontLeftEncoderInverted);
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ FrontRight =
+ ConstantCreator.createModuleConstants(
+ kFrontRightSteerMotorId,
+ kFrontRightDriveMotorId,
+ kFrontRightEncoderId,
+ kFrontRightEncoderOffset,
+ kFrontRightXPos,
+ kFrontRightYPos,
+ kInvertRightSide,
+ kFrontRightSteerMotorInverted,
+ kFrontRightEncoderInverted);
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ BackLeft =
+ ConstantCreator.createModuleConstants(
+ kBackLeftSteerMotorId,
+ kBackLeftDriveMotorId,
+ kBackLeftEncoderId,
+ kBackLeftEncoderOffset,
+ kBackLeftXPos,
+ kBackLeftYPos,
+ kInvertLeftSide,
+ kBackLeftSteerMotorInverted,
+ kBackLeftEncoderInverted);
+ public static final SwerveModuleConstants<
+ TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
+ BackRight =
+ ConstantCreator.createModuleConstants(
+ kBackRightSteerMotorId,
+ kBackRightDriveMotorId,
+ kBackRightEncoderId,
+ kBackRightEncoderOffset,
+ kBackRightXPos,
+ kBackRightYPos,
+ kInvertRightSide,
+ kBackRightSteerMotorInverted,
+ kBackRightEncoderInverted);
+
+ // /**
+ // * Creates a CommandSwerveDrivetrain instance.
+ // * This should only be called once in your robot program,.
+ // */
+ // public static CommandSwerveDrivetrain createDrivetrain() {
+ // return new CommandSwerveDrivetrain(
+ // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight
+ // );
+ // }
+
+ // /**
+ // * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
+ // */
+ // public static class TunerSwerveDrivetrain extends SwerveDrivetrain
+ // {
+ // /**
+ // * Constructs a CTRE SwerveDrivetrain using the specified constants.
+ // *
+ // * This constructs the underlying hardware devices, so users should not construct
+ // * the devices themselves. If they need the devices, they can access them through
+ // * getters in the classes.
+ // *
+ // * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+ // * @param modules Constants for each specific module
+ // */
+ // public TunerSwerveDrivetrain(
+ // SwerveDrivetrainConstants drivetrainConstants,
+ // SwerveModuleConstants... modules
+ // ) {
+ // super(
+ // TalonFX::new, TalonFX::new, CANcoder::new,
+ // drivetrainConstants, modules
+ // );
+ // }
+
+ // /**
+ // * Constructs a CTRE SwerveDrivetrain using the specified constants.
+ // *
+ // * This constructs the underlying hardware devices, so users should not construct
+ // * the devices themselves. If they need the devices, they can access them through
+ // * getters in the classes.
+ // *
+ // * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+ // * @param odometryUpdateFrequency The frequency to run the odometry loop. If
+ // * unspecified or set to 0 Hz, this is 250 Hz on
+ // * CAN FD, and 100 Hz on CAN 2.0.
+ // * @param modules Constants for each specific module
+ // */
+ // public TunerSwerveDrivetrain(
+ // SwerveDrivetrainConstants drivetrainConstants,
+ // double odometryUpdateFrequency,
+ // SwerveModuleConstants... modules
+ // ) {
+ // super(
+ // TalonFX::new, TalonFX::new, CANcoder::new,
+ // drivetrainConstants, odometryUpdateFrequency, modules
+ // );
+ // }
+
+ // /**
+ // * Constructs a CTRE SwerveDrivetrain using the specified constants.
+ // *
+ // * This constructs the underlying hardware devices, so users should not construct
+ // * the devices themselves. If they need the devices, they can access them through
+ // * getters in the classes.
+ // *
+ // * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
+ // * @param odometryUpdateFrequency The frequency to run the odometry loop. If
+ // * unspecified or set to 0 Hz, this is 250 Hz on
+ // * CAN FD, and 100 Hz on CAN 2.0.
+ // * @param odometryStandardDeviation The standard deviation for odometry calculation
+ // * in the form [x, y, theta]T, with units in meters
+ // * and radians
+ // * @param visionStandardDeviation The standard deviation for vision calculation
+ // * in the form [x, y, theta]T, with units in meters
+ // * and radians
+ // * @param modules Constants for each specific module
+ // */
+ // public TunerSwerveDrivetrain(
+ // SwerveDrivetrainConstants drivetrainConstants,
+ // double odometryUpdateFrequency,
+ // Matrix odometryStandardDeviation,
+ // Matrix visionStandardDeviation,
+ // SwerveModuleConstants... modules
+ // ) {
+ // super(
+ // TalonFX::new, TalonFX::new, CANcoder::new,
+ // drivetrainConstants, odometryUpdateFrequency,
+ // odometryStandardDeviation, visionStandardDeviation, modules
+ // );
+ // }
+ // }
+}
diff --git a/src/main/java/frc/robot/generated/DEVBOT2TunerView.java b/src/main/java/frc/robot/generated/DEVBOT2TunerView.java
new file mode 100644
index 00000000..18a905b5
--- /dev/null
+++ b/src/main/java/frc/robot/generated/DEVBOT2TunerView.java
@@ -0,0 +1,43 @@
+package frc.robot.generated;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+
+public final class DEVBOT2TunerView implements TunerView {
+ @Override
+ public CANBus canBus() {
+ return DEVBOT2TunerConstants.kCANBus;
+ }
+
+ @Override
+ public SwerveDrivetrainConstants drivetrain() {
+ return DEVBOT2TunerConstants.DrivetrainConstants;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ frontLeft() {
+ return DEVBOT2TunerConstants.FrontLeft;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ frontRight() {
+ return DEVBOT2TunerConstants.FrontRight;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ backLeft() {
+ return DEVBOT2TunerConstants.BackLeft;
+ }
+
+ @Override
+ public SwerveModuleConstants
+ backRight() {
+ return DEVBOT2TunerConstants.BackRight;
+ }
+}
diff --git a/src/main/java/frc/robot/generated/README b/src/main/java/frc/robot/generated/README
index 420d3fe4..bdaf5e8c 100644
--- a/src/main/java/frc/robot/generated/README
+++ b/src/main/java/frc/robot/generated/README
@@ -1,2 +1,69 @@
-This directory, and in particular the TunerConstants.java file, are the
-Phoenix6 equivalent of the ``src/main/deploy/swerve`` directory for YAGSL.
+Phoenix 6 generated swerve constants for RBSI
+================================================
+
+This package is the Phoenix 6 equivalent of the YAGSL `src/main/deploy/swerve`
+configuration directory.
+
+RBSI keeps one generated TunerConstants file per robot:
+
+- `COMPBOTTunerConstants.java`
+- `DEVBOT1TunerConstants.java`
+- `DEVBOT2TunerConstants.java`
+
+The files are intentionally named per robot. Phoenix Tuner X normally generates
+a file named `TunerConstants.java`; do not leave that generic name in this
+package unless you also update the RBSI view classes.
+
+How to copy a new Phoenix Tuner X file into RBSI
+------------------------------------------------
+
+1. Run the Phoenix Tuner X Swerve Project Generator for the physical robot.
+2. On the final page, choose the option that generates only `TunerConstants`.
+3. Copy the generated file into this directory:
+ `src/main/java/frc/robot/generated/`
+4. Rename it for the selected RBSI robot:
+ - `COMPBOTTunerConstants.java` for `Constants.RobotType.COMPBOT`
+ - `DEVBOT1TunerConstants.java` for `Constants.RobotType.DEVBOT1`
+ - `DEVBOT2TunerConstants.java` for `Constants.RobotType.DEVBOT2`
+5. Edit the Java class declaration inside the copied file so it matches the new
+ filename. For example:
+ `public class COMPBOTTunerConstants`
+6. Comment out the generated import for `CommandSwerveDrivetrain`. RBSI does
+ not use CTRE's generated command drivetrain class.
+7. Comment out the generated `createDrivetrain()` helper. RBSI constructs and
+ owns its drive subsystem through `frc.robot.subsystems.drive.Drive`.
+8. Leave `DrivetrainConstants`, `FrontLeft`, `FrontRight`, `BackLeft`, and
+ `BackRight` public. The RBSI view classes expose those constants to the rest
+ of the robot code.
+
+How the View classes work
+-------------------------
+
+RBSI code should not import `COMPBOTTunerConstants`, `DEVBOT1TunerConstants`, or
+`DEVBOT2TunerConstants` directly.
+
+Instead:
+
+- `TunerView` defines the small interface RBSI needs from any generated file.
+- `COMPBOTTunerView`, `DEVBOT1TunerView`, and `DEVBOT2TunerView` adapt each
+ per-robot generated file to that interface.
+- `TunerFactory` checks `Constants.getRobot()` and returns the correct view.
+- Drive code uses `TunerFactory.INSTANCE` so module IO, odometry, and
+ `SwerveConstants` all read the selected robot's generated constants.
+
+When adding another robot type, add a matching `NewRobotTunerConstants.java`, a
+`NewRobotTunerView.java`, and a `TunerFactory` switch case.
+
+Generic template values
+-----------------------
+
+The template files in this repository are not calibrated for a real robot. They
+are intentionally generic:
+
+- encoder offsets are all `Rotations.of(0.0)`,
+- module locations are `+/-10.0` inches from the robot center,
+- coupling, drive gear, steer gear, and wheel radius values match across the
+ three template files.
+
+After generating real constants, those template values should be replaced by
+the measured values from Phoenix Tuner X.
diff --git a/src/main/java/frc/robot/generated/TunerFactory.java b/src/main/java/frc/robot/generated/TunerFactory.java
new file mode 100644
index 00000000..0432f49e
--- /dev/null
+++ b/src/main/java/frc/robot/generated/TunerFactory.java
@@ -0,0 +1,21 @@
+package frc.robot.generated;
+
+import frc.robot.Constants;
+import org.littletonrobotics.junction.Logger;
+
+public final class TunerFactory {
+ public static final TunerView INSTANCE = create();
+
+ private static TunerView create() {
+ // Debugging
+ Logger.recordOutput("Drive/EncoderOffsets/RobotType", Constants.getRobot());
+
+ // Return the proper view into TunerConstants
+ return switch (Constants.getRobot()) {
+ case DEVBOT2 -> new DEVBOT2TunerView();
+ case DEVBOT1 -> new DEVBOT1TunerView();
+ case COMPBOT -> new COMPBOTTunerView();
+ default -> new COMPBOTTunerView();
+ };
+ }
+}
diff --git a/src/main/java/frc/robot/generated/TunerView.java b/src/main/java/frc/robot/generated/TunerView.java
new file mode 100644
index 00000000..8069ddcf
--- /dev/null
+++ b/src/main/java/frc/robot/generated/TunerView.java
@@ -0,0 +1,25 @@
+package frc.robot.generated;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+
+public interface TunerView {
+ CANBus canBus();
+
+ SwerveDrivetrainConstants drivetrain();
+
+ SwerveModuleConstants
+ frontLeft();
+
+ SwerveModuleConstants
+ frontRight();
+
+ SwerveModuleConstants
+ backLeft();
+
+ SwerveModuleConstants
+ backRight();
+}
diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java
index e6ebe08d..5b7b4e25 100644
--- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java
+++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java
@@ -16,6 +16,7 @@
import edu.wpi.first.math.geometry.Translation3d;
import frc.robot.Constants;
import frc.robot.Constants.RobotConstants;
+import frc.robot.Constants.SensorConstants;
import frc.robot.subsystems.imu.Imu;
import frc.robot.util.TimeUtil;
import frc.robot.util.VirtualSubsystem;
@@ -37,7 +38,7 @@ public class Accelerometer extends VirtualSubsystem {
// Variables needed during the periodic
private Translation3d rawRio, rioAcc, rioJerk, imuAcc, imuJerk;
- private Translation3d prevRioAcc = Translation3d.kZero;
+ private Translation3d prevRioAcc = null;
// Log decimation
private int loopCount = 0;
@@ -48,8 +49,12 @@ public class Accelerometer extends VirtualSubsystem {
private static final int PROFILE_EVERY_N = 50; // 1Hz profiling
public Accelerometer(Imu imu) {
+ this(imu, new RioAccelIORoboRIO(SensorConstants.kRioAccelerometerSampleRateHz));
+ }
+
+ public Accelerometer(Imu imu, RioAccelIO rio) {
this.imu = imu;
- this.rio = new RioAccelIORoboRIO(200.0); // 200 Hz is a good start
+ this.rio = rio;
}
// Priority value for this virtual subsystem
@@ -70,12 +75,15 @@ public void rbsiPeriodic() {
// Compute RIO accelerations and jerks
rawRio =
new Translation3d(
- rioInputs.xG * Constants.G_TO_MPS2,
- rioInputs.yG * Constants.G_TO_MPS2,
- rioInputs.zG * Constants.G_TO_MPS2);
+ rioInputs.xG * Constants.kGravityMetersPerSecSq,
+ rioInputs.yG * Constants.kGravityMetersPerSecSq,
+ rioInputs.zG * Constants.kGravityMetersPerSecSq);
rioAcc = rawRio.rotateBy(RobotConstants.kRioOrientation);
- // Acceleration from previous loop
+ Translation3d rioJerkThisLoop =
+ prevRioAcc == null
+ ? Translation3d.kZero
+ : rioAcc.minus(prevRioAcc).div(Constants.kLoopPeriodSecs);
prevRioAcc = rioAcc;
// IMU accelerations and jerks
@@ -89,7 +97,7 @@ public void rbsiPeriodic() {
final boolean doHeavyLogs = (++loopCount >= LOG_EVERY_N);
if (doHeavyLogs) {
loopCount = 0;
- rioJerk = rioAcc.minus(prevRioAcc).div(Constants.loopPeriodSecs);
+ rioJerk = rioJerkThisLoop;
imuJerk = imuInputs.linearJerk.rotateBy(RobotConstants.kIMUOrientation);
Logger.recordOutput("Accel/Rio/Jerk_mps3", rioJerk);
Logger.recordOutput("Accel/IMU/Jerk_mps3", imuJerk);
diff --git a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java
index 64fad53a..2a60a893 100644
--- a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java
+++ b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java
@@ -23,4 +23,17 @@ final class Inputs {
default void updateInputs(Inputs inputs) {}
default void close() {}
+
+ static RioAccelIO noop() {
+ return new RioAccelIO() {
+ @Override
+ public void updateInputs(Inputs inputs) {
+ inputs.connected = false;
+ inputs.timestampNs = 0L;
+ inputs.xG = 0.0;
+ inputs.yG = 0.0;
+ inputs.zG = 0.0;
+ }
+ };
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index c6e36a5a..ea54345d 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -21,7 +21,6 @@
import static frc.robot.subsystems.drive.SwerveConstants.*;
import choreo.trajectory.SwerveSample;
-import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.config.PIDConstants;
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
@@ -30,6 +29,7 @@
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -45,6 +45,8 @@
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;
@@ -81,11 +83,13 @@ public class Drive extends RBSISubsystem {
// Pose Buffer Declarations
private final ConcurrentTimeInterpolatableBuffer poseBuffer =
- ConcurrentTimeInterpolatableBuffer.createBuffer(DrivebaseConstants.kHistorySize);
+ ConcurrentTimeInterpolatableBuffer.createBuffer(DrivebaseConstants.kPoseBufferHistorySecs);
private final ConcurrentTimeInterpolatableBuffer yawBuffer =
- ConcurrentTimeInterpolatableBuffer.createDoubleBuffer(DrivebaseConstants.kHistorySize);
+ ConcurrentTimeInterpolatableBuffer.createDoubleBuffer(
+ DrivebaseConstants.kPoseBufferHistorySecs);
private final ConcurrentTimeInterpolatableBuffer yawRateBuffer =
- ConcurrentTimeInterpolatableBuffer.createDoubleBuffer(DrivebaseConstants.kHistorySize);
+ ConcurrentTimeInterpolatableBuffer.createDoubleBuffer(
+ DrivebaseConstants.kPoseBufferHistorySecs);
// Declare an alert
private final Alert gyroDisconnectedAlert =
@@ -109,6 +113,7 @@ public class Drive extends RBSISubsystem {
// Declare PID controller and siumulation physics
private ProfiledPIDController angleController;
private DriveSimPhysics simPhysics;
+ private final Field2d field = new Field2d();
// Pose reset gate (vision + anything latency-sensitive)
private volatile long poseResetEpoch = 0; // monotonic counter
@@ -134,12 +139,13 @@ public Drive(Imu imu) {
// Define the Angle Controller
angleController =
new ProfiledPIDController(
- DrivebaseConstants.kPSPin,
- DrivebaseConstants.kISPin,
- DrivebaseConstants.kDSpin,
+ DrivebaseConstants.kSpinP,
+ DrivebaseConstants.kSpinI,
+ DrivebaseConstants.kSpinD,
new TrapezoidProfile.Constraints(
- getMaxAngularSpeedRadPerSec(), getMaxLinearAccelMetersPerSecPerSec()));
+ getMaxAngularSpeedRadPerSec(), getMaxAngularAccelRadPerSecPerSec()));
angleController.enableContinuousInput(-Math.PI, Math.PI);
+ m_pathThetaController.enableContinuousInput(-Math.PI, Math.PI);
// If REAL (i.e., NOT simulation), parse out the module types
if (Constants.getMode() == Mode.REAL) {
@@ -165,6 +171,7 @@ public Drive(Imu imu) {
throw new RuntimeException(
"For an all-CTRE drive base, use Phoenix Tuner X Swerve Generator instead of YAGSL!");
}
+ break;
case 0b00010000: // Blended Talon Drive / NEO Steer
modules[i] = new Module(new ModuleIOBlended(i), i);
break;
@@ -186,6 +193,7 @@ public Drive(Imu imu) {
// Start odometry thread (for the real robot)
PhoenixOdometryThread.getInstance().start();
+ SparkOdometryThread.getInstance().start();
} else {
@@ -198,8 +206,8 @@ public Drive(Imu imu) {
simPhysics =
new DriveSimPhysics(
kinematics,
- RobotConstants.kRobotMOI, // kg m^2
- RobotConstants.kMaxWheelTorque); // Nm
+ RobotConstants.kMomentOfInertiaKgMetersSq, // kg m^2
+ RobotConstants.kMaxWheelTorqueNm); // Nm
}
// Usage reporting for swerve template
@@ -217,13 +225,13 @@ public Drive(Imu imu) {
(speeds, feedforwards) -> runVelocity(speeds),
new PPHolonomicDriveController(
new PIDConstants(
- DrivebaseConstants.kPStrafe,
- DrivebaseConstants.kIStrafe,
- DrivebaseConstants.kDStrafe),
+ DrivebaseConstants.kStrafeP,
+ DrivebaseConstants.kStrafeI,
+ DrivebaseConstants.kStrafeD),
new PIDConstants(
- DrivebaseConstants.kPSPin,
- DrivebaseConstants.kISPin,
- DrivebaseConstants.kDSpin)),
+ DrivebaseConstants.kSpinP,
+ DrivebaseConstants.kSpinI,
+ DrivebaseConstants.kSpinD)),
AutoConstants.kPathPlannerConfig,
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
this);
@@ -243,7 +251,7 @@ public Drive(Imu imu) {
break;
case CHOREO:
- // TODO: If your team is using Choreo, you'll know what to do here...
+ // Choreo autos are configured in RobotContainer through AutoFactory.
break;
case MANUAL:
@@ -262,6 +270,8 @@ public Drive(Imu imu) {
(state) -> Logger.recordOutput("Drive/SysIdState", state.toString())),
new SysIdRoutine.Mechanism(
(voltage) -> runCharacterization(voltage.in(Volts)), null, this));
+
+ SmartDashboard.putData("Field", field);
}
/************************************************************************* */
@@ -276,6 +286,8 @@ public void rbsiPeriodic() {
Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
}
+
+ field.setRobotPose(m_PoseEstimator.getEstimatedPosition());
}
/**
@@ -291,7 +303,7 @@ public void simulationPeriodic() {
// IMPORTANT: do not run sim physics during REPLAY
if (Constants.getMode() != Mode.SIM) return;
- final double dt = Constants.loopPeriodSecs;
+ final double dt = Constants.kLoopPeriodSecs;
// Advance module wheel physics
for (int i = 0; i < modules.length; i++) {
@@ -366,7 +378,7 @@ public void stopWithX() {
*/
public void runVelocity(ChassisSpeeds speeds) {
// Calculate module setpoints
- ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, Constants.loopPeriodSecs);
+ ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, Constants.kLoopPeriodSecs);
SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, getMaxLinearSpeedMetersPerSec());
@@ -425,8 +437,8 @@ public void updateDisabledCoastState(
SwerveModulePosition[] odomPositions) {
// Don’t end coast “instantly” right after disable edge
- final double minCoastTime = 0.25; // seconds -- maybe put into Constants???
- final boolean pastMin = (now - disabledCoastStartTs) >= minCoastTime;
+ final boolean pastMin =
+ (now - disabledCoastStartTs) >= DrivebaseConstants.kDisabledCoastMinSeconds;
// Detect ENABLED -> DISABLED edge -- set `disabledCoastUntilTs` when COAST-phase ends
if (lastEnabled && !enabledNow) {
@@ -455,8 +467,10 @@ public void updateDisabledCoastState(
return;
}
- // Compute max wheel delta this loop
+ // Compute max wheel delta this loop. The first sample after a reset only establishes the
+ // baseline and should not count as stationary.
double maxDelta = 0.0;
+ boolean hadLastWheelDist = haveLastWheelDist;
if (haveLastWheelDist) {
for (int i = 0; i < 4; i++) {
double dist = odomPositions[i].distanceMeters;
@@ -472,7 +486,7 @@ public void updateDisabledCoastState(
haveLastWheelDist = true;
// Stationary test (must have baseline)
- if (haveLastWheelDist
+ if (hadLastWheelDist
&& maxDelta <= DrivebaseConstants.kStationaryMaxWheelDeltaM
&& Math.abs(yawRateRadPerSec) <= DrivebaseConstants.kStationaryMaxYawRateRadPerSec) {
stationaryLoops++;
@@ -499,13 +513,15 @@ public void updateDisabledCoastState(
/** Returns a command to run a quasistatic test in the specified direction. */
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return run(() -> runCharacterization(0.0))
- .withTimeout(1.0)
+ .withTimeout(DrivebaseConstants.kSysIdPreRunStopSecs)
.andThen(sysId.quasistatic(direction));
}
/** Returns a command to run a dynamic test in the specified direction. */
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
- return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction));
+ return run(() -> runCharacterization(0.0))
+ .withTimeout(DrivebaseConstants.kSysIdPreRunStopSecs)
+ .andThen(sysId.dynamic(direction));
}
/************************************************************************* */
@@ -600,12 +616,12 @@ public Optional getPoseAtTime(double timestampSeconds) {
/** Returns the oldest timetamp in the current pose buffer */
public double getPoseBufferOldestTime() {
- return poseBuffer.getOldestTimestamp().getAsDouble();
+ return poseBuffer.getOldestTimestamp().orElse(Double.NaN);
}
/** Returns the newest timetamp in the current pose buffer */
public double getPoseBufferNewestTime() {
- return poseBuffer.getNewestTimestamp().getAsDouble();
+ return poseBuffer.getNewestTimestamp().orElse(Double.NaN);
}
/**
@@ -620,7 +636,7 @@ public OptionalDouble getMaxAbsYawRateRadPerSec(double t0, double t1) {
if (t1 < t0) return OptionalDouble.empty();
// Get the subset of entries from the buffer
- var sub = yawRateBuffer.getInternalBuffer().subMap(t0, true, t1, true);
+ var sub = yawRateBuffer.getSamplesInRange(t0, true, t1, true);
if (sub.isEmpty()) return OptionalDouble.empty();
double maxAbs = 0.0;
@@ -646,7 +662,7 @@ public double getLastPoseResetTimestamp() {
/** Returns the maximum linear speed in meters per sec. */
public double getMaxLinearSpeedMetersPerSec() {
- return DrivebaseConstants.kMaxLinearSpeed;
+ return DrivebaseConstants.kMaxLinearSpeedMetersPerSec;
}
/** Returns the maximum angular speed in radians per sec. */
@@ -656,7 +672,7 @@ public double getMaxAngularSpeedRadPerSec() {
/** Returns the maximum linear acceleration in meters per sec per sec. */
public double getMaxLinearAccelMetersPerSecPerSec() {
- return DrivebaseConstants.kMaxLinearAccel;
+ return DrivebaseConstants.kMaxLinearAccelMetersPerSecSq;
}
/** Returns the maximum angular acceleration in radians per sec per sec */
@@ -778,7 +794,9 @@ public void addVisionMeasurement(TimedPose meas) {
// If we're coasting, avoid snapping Pose to Vision; lean gentler than stationary.
final double alpha =
coast
- ? Math.min(DrivebaseConstants.kDisabledVisionBlendAlpha, 0.05)
+ ? Math.min(
+ DrivebaseConstants.kDisabledVisionBlendAlpha,
+ DrivebaseConstants.kDisabledVisionCoastBlendAlpha)
: DrivebaseConstants.kDisabledVisionBlendAlpha;
// "Current" for blending target (estimator pose)
@@ -912,7 +930,7 @@ void yawBuffersFillFromQueue(double[] yawTs, double[] yawPosRad) {
if (k > 0) {
double dt = yawTs[k] - yawTs[k - 1];
if (dt > 1e-6) {
- yawRateBuffer.addSample(yawTs[k], (yawPosRad[k] - yawPosRad[k - 1]) / dt);
+ yawRateBuffer.addSample(yawTs[k], yawRateRadPerSec(yawPosRad[k - 1], yawPosRad[k], dt));
}
}
}
@@ -924,11 +942,16 @@ void yawBuffersAddSampleIndexAligned(double t, double[] yawTs, double[] yawPos,
if (i > 0) {
double dt = yawTs[i] - yawTs[i - 1];
if (dt > 1e-6) {
- yawRateBuffer.addSample(t, (yawPos[i] - yawPos[i - 1]) / dt);
+ yawRateBuffer.addSample(t, yawRateRadPerSec(yawPos[i - 1], yawPos[i], dt));
}
}
}
+ static double yawRateRadPerSec(double previousYawRad, double currentYawRad, double dtSec) {
+ if (dtSec <= 1e-6) return 0.0;
+ return MathUtil.angleModulus(currentYawRad - previousYawRad) / dtSec;
+ }
+
/** Set the gyroDisconnectedAlert */
void setGyroDisconnectedAlert(boolean disconnected) {
gyroDisconnectedAlert.set(disconnected);
@@ -952,20 +975,26 @@ public double getSimYawRateRadPerSec() {
/** CHOREO SECTION (Ignore if AutoType == PATHPLANNER) ******************* */
/** Choreo: Reset odometry */
- public Command resetOdometry(Pose2d orElseGet) {
- // TODO Auto-generated method stub
- throw new UnsupportedOperationException("Unimplemented method 'resetOdometry'");
+ public void resetOdometry(Pose2d pose) {
+ resetPose(pose);
}
- /** Swerve request to apply during field-centric path following */
- @SuppressWarnings("unused")
- private final SwerveRequest.ApplyFieldSpeeds m_pathApplyFieldSpeeds =
- new SwerveRequest.ApplyFieldSpeeds();
-
// Choreo Controller Values
- private final PIDController m_pathXController = new PIDController(10, 0, 0);
- private final PIDController m_pathYController = new PIDController(10, 0, 0);
- private final PIDController m_pathThetaController = new PIDController(7, 0, 0);
+ private final PIDController m_pathXController =
+ new PIDController(
+ AutoConstants.kChoreoDrivePID.kP,
+ AutoConstants.kChoreoDrivePID.kI,
+ AutoConstants.kChoreoDrivePID.kD);
+ private final PIDController m_pathYController =
+ new PIDController(
+ AutoConstants.kChoreoDrivePID.kP,
+ AutoConstants.kChoreoDrivePID.kI,
+ AutoConstants.kChoreoDrivePID.kD);
+ private final PIDController m_pathThetaController =
+ new PIDController(
+ AutoConstants.kChoreoSteerPID.kP,
+ AutoConstants.kChoreoSteerPID.kI,
+ AutoConstants.kChoreoSteerPID.kD);
/**
* Follows the given field-centric path sample with PID for Choreo
@@ -974,34 +1003,28 @@ public Command resetOdometry(Pose2d orElseGet) {
* @param sample Sample along the path to follow
*/
public void choreoController(Pose2d pose, SwerveSample sample) {
- m_pathThetaController.enableContinuousInput(-Math.PI, Math.PI);
-
var targetSpeeds = sample.getChassisSpeeds();
targetSpeeds.vxMetersPerSecond += m_pathXController.calculate(pose.getX(), sample.x);
targetSpeeds.vyMetersPerSecond += m_pathYController.calculate(pose.getY(), sample.y);
targetSpeeds.omegaRadiansPerSecond +=
m_pathThetaController.calculate(pose.getRotation().getRadians(), sample.heading);
- // setControl(
- // m_pathApplyFieldSpeeds
- // .withSpeeds(targetSpeeds)
- // .withWheelForceFeedforwardsX(sample.moduleForcesX())
- // .withWheelForceFeedforwardsY(sample.moduleForcesY()));
+ runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(targetSpeeds, getHeading()));
}
public void followTrajectory(SwerveSample sample) {
// Get the current pose of the robot
Pose2d pose = getPose();
- // Generate the next speeds for the robot
- ChassisSpeeds speeds =
+ // Choreo samples are field-relative; convert to robot-relative before sending to modules.
+ ChassisSpeeds fieldRelativeSpeeds =
new ChassisSpeeds(
sample.vx + m_pathXController.calculate(pose.getX(), sample.x),
- sample.vy + m_pathXController.calculate(pose.getX(), sample.y),
+ sample.vy + m_pathYController.calculate(pose.getY(), sample.y),
sample.omega
- + m_pathXController.calculate(pose.getRotation().getRadians(), sample.heading));
+ + m_pathThetaController.calculate(pose.getRotation().getRadians(), sample.heading));
// Apply the generated speeds
- runVelocity(speeds);
+ runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, pose.getRotation()));
}
}
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java
index 7dc4925b..6f4b49bb 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveOdometry.java
@@ -187,6 +187,7 @@ public void rbsiPeriodic() {
// ----------------------------------------------------------------------
final double[] lastDist = new double[4];
boolean haveLastDist = false;
+ final boolean logDebug = Constants.isTuningMode();
for (int i = 0; i < n; i++) {
final double t = ts[i];
@@ -224,26 +225,31 @@ public void rbsiPeriodic() {
imuInputs.yawRateRadPerSec,
odomPositions);
- // Debugging
- Logger.recordOutput("Odometry/Debug/timestamp", t);
- Logger.recordOutput("Odometry/Debug/now", TimeUtil.now());
- if (i > 0) {
- Logger.recordOutput("Odometry/Debug/timeNowDiff", t - ts[i - 1]);
+ if (logDebug && i == n - 1) {
+ Logger.recordOutput("Odometry/Debug/timestamp", t);
+ Logger.recordOutput("Odometry/Debug/now", TimeUtil.now());
+ if (i > 0) {
+ Logger.recordOutput("Odometry/Debug/timeNowDiff", t - ts[i - 1]);
+ }
+ Logger.recordOutput("Odometry/Debug/replay_t", t);
+ Logger.recordOutput("Odometry/Debug/replay_yawRad", yawRad);
}
- Logger.recordOutput("Odometry/Debug/replay_t", t);
- Logger.recordOutput("Odometry/Debug/replay_yawRad", yawRad);
// Module distance deltas (valid within batch)
for (int m = 0; m < 4; m++) {
final SwerveModulePosition pos = odomPositions[m];
final double dist = pos.distanceMeters;
- Logger.recordOutput("Odometry/Debug/mod" + m + "_distanceMeters", dist);
- Logger.recordOutput("Odometry/Debug/mod" + m + "_angleRad", pos.angle.getRadians());
+ if (logDebug && i == n - 1) {
+ Logger.recordOutput("Odometry/Debug/mod" + m + "_distanceMeters", dist);
+ Logger.recordOutput("Odometry/Debug/mod" + m + "_angleRad", pos.angle.getRadians());
+ }
if (haveLastDist) {
final double delta = dist - lastDist[m];
- Logger.recordOutput("Odometry/Debug/mod" + m + "_deltaMeters", delta);
+ if (logDebug && i == n - 1) {
+ Logger.recordOutput("Odometry/Debug/mod" + m + "_deltaMeters", delta);
+ }
}
lastDist[m] = dist;
diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java
index b57653d8..87db48e6 100644
--- a/src/main/java/frc/robot/subsystems/drive/Module.java
+++ b/src/main/java/frc/robot/subsystems/drive/Module.java
@@ -25,6 +25,8 @@
* the robot. This is not a true subsystem, but an abstraction layer.
*/
public class Module {
+ private static final SwerveModulePosition[] EMPTY_ODOMETRY_POSITIONS =
+ new SwerveModulePosition[0];
// Define IO
private final ModuleIO io;
@@ -35,7 +37,7 @@ public class Module {
private final Alert driveDisconnectedAlert;
private final Alert turnDisconnectedAlert;
private final Alert turnEncoderDisconnectedAlert;
- private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
+ private SwerveModulePosition[] odometryPositions = EMPTY_ODOMETRY_POSITIONS;
/** Constructor */
public Module(ModuleIO io, int index) {
@@ -61,7 +63,17 @@ public void periodic() {
Logger.processInputs("Drive/Module" + Integer.toString(index), inputs);
// Calculate positions for odometry
- int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
+ int sampleCount =
+ Math.min(
+ inputs.odometryTimestamps.length,
+ Math.min(inputs.odometryDrivePositionsRad.length, inputs.odometryTurnPositions.length));
+ if (sampleCount <= 0) {
+ odometryPositions = EMPTY_ODOMETRY_POSITIONS;
+ driveDisconnectedAlert.set(!inputs.driveConnected);
+ turnDisconnectedAlert.set(!inputs.turnConnected);
+ turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected);
+ return;
+ }
odometryPositions = new SwerveModulePosition[sampleCount];
for (int i = 0; i < sampleCount; i++) {
double positionMeters =
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
index 10055776..7affb4f5 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java
@@ -13,6 +13,9 @@
import org.littletonrobotics.junction.AutoLog;
public interface ModuleIO {
+ double[] EMPTY_DOUBLE_ARRAY = new double[0];
+ Rotation2d[] EMPTY_ROTATION_ARRAY = new Rotation2d[0];
+
@AutoLog
public static class ModuleIOInputs {
public boolean driveConnected = false;
@@ -29,9 +32,9 @@ public static class ModuleIOInputs {
public double turnAppliedVolts = 0.0;
public double turnCurrentAmps = 0.0;
- public double[] odometryTimestamps = new double[] {};
- public double[] odometryDrivePositionsRad = new double[] {};
- public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};
+ public double[] odometryTimestamps = EMPTY_DOUBLE_ARRAY;
+ public double[] odometryDrivePositionsRad = EMPTY_DOUBLE_ARRAY;
+ public Rotation2d[] odometryTurnPositions = EMPTY_ROTATION_ARRAY;
}
/** Updates the set of loggable inputs. */
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java
index 9ae8105b..8ddb7138 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java
@@ -19,7 +19,6 @@
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.TorqueCurrentFOC;
import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
@@ -95,7 +94,6 @@ public class ModuleIOBlended implements ModuleIO {
private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0);
// Torque-current control requests
- private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0);
private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest =
new VelocityTorqueCurrentFOC(0.0);
@@ -208,19 +206,19 @@ public ModuleIOBlended(int module) {
.withKV(DrivebaseConstants.kDriveV)
.withKA(DrivebaseConstants.kDriveA);
driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio;
- driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DrivebaseConstants.kSlipCurrent;
- driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DrivebaseConstants.kSlipCurrent;
+ driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DrivebaseConstants.kSlipCurrentAmps;
+ driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DrivebaseConstants.kSlipCurrentAmps;
driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit;
driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
// Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing
OpenLoopRampsConfigs openRamps = new OpenLoopRampsConfigs();
- openRamps.DutyCycleOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod;
- openRamps.VoltageOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod;
- openRamps.TorqueOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod;
+ openRamps.DutyCycleOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriodSecs;
+ openRamps.VoltageOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriodSecs;
+ openRamps.TorqueOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriodSecs;
ClosedLoopRampsConfigs closedRamps = new ClosedLoopRampsConfigs();
- closedRamps.DutyCycleClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod;
- closedRamps.VoltageClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod;
- closedRamps.TorqueClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod;
+ closedRamps.DutyCycleClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriodSecs;
+ closedRamps.VoltageClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriodSecs;
+ closedRamps.TorqueClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriodSecs;
// Apply the open- and closed-loop ramp configuration for current smoothing
driveConfig.withClosedLoopRamps(closedRamps).withOpenLoopRamps(openRamps);
// Set motor inversions
@@ -234,7 +232,7 @@ public ModuleIOBlended(int module) {
.inverted(constants.SteerMotorInverted)
.idleMode(IdleMode.kBrake)
.smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit)
- .voltageCompensation(DrivebaseConstants.kOptimalVoltage);
+ .voltageCompensation(DrivebaseConstants.kNominalVoltage);
turnConfig
.absoluteEncoder
.inverted(constants.EncoderInverted)
@@ -256,13 +254,13 @@ public ModuleIOBlended(int module) {
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency))
.absoluteEncoderVelocityAlwaysOn(true)
- .absoluteEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .busVoltagePeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .outputCurrentPeriodMs((int) (Constants.loopPeriodSecs * 1000.));
+ .absoluteEncoderVelocityPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .appliedOutputPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .busVoltagePeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .outputCurrentPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.));
turnConfig
- .openLoopRampRate(DrivebaseConstants.kDriveOpenLoopRampPeriod)
- .closedLoopRampRate(DrivebaseConstants.kDriveClosedLoopRampPeriod);
+ .openLoopRampRate(DrivebaseConstants.kDriveOpenLoopRampPeriodSecs)
+ .closedLoopRampRate(DrivebaseConstants.kDriveClosedLoopRampPeriodSecs);
// Configure CANCoder
CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs;
@@ -354,9 +352,9 @@ public void updateInputs(ModuleIOInputs inputs) {
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];
+ inputs.odometryTimestamps = EMPTY_DOUBLE_ARRAY;
+ inputs.odometryDrivePositionsRad = EMPTY_DOUBLE_ARRAY;
+ inputs.odometryTurnPositions = EMPTY_ROTATION_ARRAY;
return;
}
@@ -395,13 +393,9 @@ public void updateInputs(ModuleIOInputs inputs) {
public void setDriveOpenLoop(double output) {
// Scale by actual battery voltage to keep full output consistent
double busVoltage = RobotController.getBatteryVoltage();
- double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double scaledOutput = output * DrivebaseConstants.kNominalVoltage / busVoltage;
- driveTalon.setControl(
- switch (m_DriveMotorClosedLoopOutput) {
- case Voltage -> voltageRequest.withOutput(scaledOutput);
- case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(scaledOutput);
- });
+ driveTalon.setControl(voltageRequest.withOutput(scaledOutput));
// Log output and battery
Logger.recordOutput("Swerve/Drive/OpenLoopOutput", scaledOutput);
@@ -416,7 +410,7 @@ public void setDriveOpenLoop(double output) {
@Override
public void setTurnOpenLoop(double output) {
double busVoltage = RobotController.getBatteryVoltage();
- double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double scaledOutput = output * DrivebaseConstants.kNominalVoltage / busVoltage;
turnSpark.setVoltage(scaledOutput);
// Log output and battery
@@ -448,7 +442,7 @@ public void setDriveVelocity(double velocityRadPerSec) {
Math.signum(velocityRotPerSec) * DrivebaseConstants.kDriveS
+ DrivebaseConstants.kDriveV * velocityRotPerSec
+ DrivebaseConstants.kDriveA * accelerationRotPerSec2;
- double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kNominalVoltage / busVoltage;
// Set the drive motor control based on CTRE LICENSED status
driveTalon.setControl(
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
index ec87f59e..740d42e2 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java
@@ -75,7 +75,7 @@ public void simulationPeriodic() {
}
if (turnClosedLoop) {
- // TODO: turn PID has no feedforward or inertia compensation; fix
+ // Simple angle loop for module simulation; real feedforward is handled by hardware IO.
turnAppliedVolts = turnController.calculate(turnSim.getAngularPositionRad());
} else {
turnController.reset();
@@ -86,8 +86,8 @@ public void simulationPeriodic() {
turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -12.0, 12.0));
// Advance physics
- driveSim.update(Constants.loopPeriodSecs);
- turnSim.update(Constants.loopPeriodSecs);
+ driveSim.update(Constants.kLoopPeriodSecs);
+ turnSim.update(Constants.kLoopPeriodSecs);
}
@Override
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java
index c79d5ee9..0fc8c2cc 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java
@@ -129,7 +129,7 @@ public ModuleIOSpark(int module) {
driveConfig
.idleMode(IdleMode.kBrake)
.smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit)
- .voltageCompensation(DrivebaseConstants.kOptimalVoltage);
+ .voltageCompensation(DrivebaseConstants.kNominalVoltage);
driveConfig
.encoder
.positionConversionFactor(SwerveConstants.driveEncoderPositionFactor)
@@ -148,13 +148,13 @@ public ModuleIOSpark(int module) {
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency))
.primaryEncoderVelocityAlwaysOn(true)
- .primaryEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .busVoltagePeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .outputCurrentPeriodMs((int) (Constants.loopPeriodSecs * 1000.));
+ .primaryEncoderVelocityPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .appliedOutputPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .busVoltagePeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .outputCurrentPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.));
driveConfig
- .openLoopRampRate(DrivebaseConstants.kDriveOpenLoopRampPeriod)
- .closedLoopRampRate(DrivebaseConstants.kDriveClosedLoopRampPeriod);
+ .openLoopRampRate(DrivebaseConstants.kDriveOpenLoopRampPeriodSecs)
+ .closedLoopRampRate(DrivebaseConstants.kDriveClosedLoopRampPeriodSecs);
SparkUtil.tryUntilOk(
driveSpark,
5,
@@ -169,7 +169,7 @@ public ModuleIOSpark(int module) {
.inverted(turnInverted)
.idleMode(IdleMode.kBrake)
.smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit)
- .voltageCompensation(DrivebaseConstants.kOptimalVoltage);
+ .voltageCompensation(DrivebaseConstants.kNominalVoltage);
turnConfig
.absoluteEncoder
.inverted(turnEncoderInverted)
@@ -191,13 +191,13 @@ public ModuleIOSpark(int module) {
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency))
.absoluteEncoderVelocityAlwaysOn(true)
- .absoluteEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .busVoltagePeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .outputCurrentPeriodMs((int) (Constants.loopPeriodSecs * 1000.));
+ .absoluteEncoderVelocityPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .appliedOutputPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .busVoltagePeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .outputCurrentPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.));
turnConfig
- .openLoopRampRate(DrivebaseConstants.kDriveOpenLoopRampPeriod)
- .closedLoopRampRate(DrivebaseConstants.kDriveClosedLoopRampPeriod);
+ .openLoopRampRate(DrivebaseConstants.kDriveOpenLoopRampPeriodSecs)
+ .closedLoopRampRate(DrivebaseConstants.kDriveClosedLoopRampPeriodSecs);
SparkUtil.tryUntilOk(
turnSpark,
5,
@@ -256,9 +256,9 @@ public void updateInputs(ModuleIOInputs inputs) {
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];
+ inputs.odometryTimestamps = EMPTY_DOUBLE_ARRAY;
+ inputs.odometryDrivePositionsRad = EMPTY_DOUBLE_ARRAY;
+ inputs.odometryTurnPositions = EMPTY_ROTATION_ARRAY;
return;
}
@@ -337,7 +337,7 @@ public void setDriveVelocity(double velocityRadPerSec) {
+ DrivebaseConstants.kDriveA * accelerationRadPerSec2;
double busVoltage = RobotController.getBatteryVoltage();
- double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kNominalVoltage / busVoltage;
driveController.setSetpoint(
velocityRadPerSec,
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java
index 7c106838..743bb495 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java
@@ -158,7 +158,7 @@ public ModuleIOSparkCANcoder(int module) {
driveConfig
.idleMode(IdleMode.kBrake)
.smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit)
- .voltageCompensation(DrivebaseConstants.kOptimalVoltage);
+ .voltageCompensation(DrivebaseConstants.kNominalVoltage);
driveConfig
.encoder
.positionConversionFactor(SwerveConstants.driveEncoderPositionFactor)
@@ -195,7 +195,7 @@ public ModuleIOSparkCANcoder(int module) {
.inverted(turnInverted)
.idleMode(IdleMode.kBrake)
.smartCurrentLimit((int) SwerveConstants.kSteerCurrentLimit)
- .voltageCompensation(DrivebaseConstants.kOptimalVoltage);
+ .voltageCompensation(DrivebaseConstants.kNominalVoltage);
turnConfig
.absoluteEncoder
.inverted(turnEncoderInverted)
@@ -217,13 +217,13 @@ public ModuleIOSparkCANcoder(int module) {
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency))
.absoluteEncoderVelocityAlwaysOn(true)
- .absoluteEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .busVoltagePeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .outputCurrentPeriodMs((int) (Constants.loopPeriodSecs * 1000.));
+ .absoluteEncoderVelocityPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .appliedOutputPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .busVoltagePeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .outputCurrentPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.));
turnConfig
- .openLoopRampRate(DrivebaseConstants.kDriveOpenLoopRampPeriod)
- .closedLoopRampRate(DrivebaseConstants.kDriveClosedLoopRampPeriod);
+ .openLoopRampRate(DrivebaseConstants.kDriveOpenLoopRampPeriodSecs)
+ .closedLoopRampRate(DrivebaseConstants.kDriveClosedLoopRampPeriodSecs);
SparkUtil.tryUntilOk(
turnSpark,
5,
@@ -235,18 +235,25 @@ public ModuleIOSparkCANcoder(int module) {
turnVelocity = cancoder.getVelocity();
turnAbsolutePosition = cancoder.getAbsolutePosition();
turnPosition = cancoder.getPosition();
+ BaseStatusSignal.setUpdateFrequencyForAll(SwerveConstants.kOdometryFrequency, turnPosition);
// Create odometry queues
timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
drivePositionQueue =
SparkOdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition);
- turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(cancoder.getPosition());
+ turnPositionQueue =
+ SparkOdometryThread.getInstance()
+ .registerSignal(
+ () -> {
+ turnPosition.refresh();
+ return turnPosition.getValueAsDouble();
+ });
}
@Override
public void updateInputs(ModuleIOInputs inputs) {
// Refresh CANcoder absolute
- var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition);
+ var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition, turnPosition, turnVelocity);
if (!encStatus.isOK()) {
Logger.recordOutput("CAN/Module" + module + "/EncRefreshStatus", encStatus.toString());
}
@@ -286,9 +293,9 @@ public void updateInputs(ModuleIOInputs inputs) {
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];
+ inputs.odometryTimestamps = EMPTY_DOUBLE_ARRAY;
+ inputs.odometryDrivePositionsRad = EMPTY_DOUBLE_ARRAY;
+ inputs.odometryTurnPositions = EMPTY_ROTATION_ARRAY;
return;
}
@@ -326,7 +333,7 @@ public void updateInputs(ModuleIOInputs inputs) {
@Override
public void setDriveOpenLoop(double output) {
double busVoltage = RobotController.getBatteryVoltage();
- double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double scaledOutput = output * DrivebaseConstants.kNominalVoltage / busVoltage;
driveSpark.setVoltage(scaledOutput);
// Log output and battery
@@ -342,7 +349,7 @@ public void setDriveOpenLoop(double output) {
@Override
public void setTurnOpenLoop(double output) {
double busVoltage = RobotController.getBatteryVoltage();
- double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double scaledOutput = output * DrivebaseConstants.kNominalVoltage / busVoltage;
turnSpark.setVoltage(scaledOutput);
// Log output and battery
@@ -373,7 +380,7 @@ public void setDriveVelocity(double velocityRadPerSec) {
+ DrivebaseConstants.kDriveA * accelerationRadPerSec2;
double busVoltage = RobotController.getBatteryVoltage();
- double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kNominalVoltage / busVoltage;
driveController.setSetpoint(
velocityRadPerSec,
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
index ccf9c270..802503bb 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -21,7 +21,6 @@
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.TorqueCurrentFOC;
import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
@@ -45,7 +44,7 @@
import edu.wpi.first.wpilibj.RobotController;
import frc.robot.Constants;
import frc.robot.Constants.DrivebaseConstants;
-import frc.robot.generated.TunerConstants;
+import frc.robot.generated.TunerFactory;
import frc.robot.util.PhoenixUtil;
import frc.robot.util.RBSICANBusRegistry;
import java.util.Arrays;
@@ -87,7 +86,6 @@ public class ModuleIOTalonFX implements ModuleIO {
private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0);
// Torque-current control requests
- private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0);
private final PositionTorqueCurrentFOC positionTorqueCurrentRequest =
new PositionTorqueCurrentFOC(0.0);
private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest =
@@ -138,10 +136,10 @@ public ModuleIOTalonFX(int module) {
constants =
switch (module) {
- case 0 -> TunerConstants.FrontLeft;
- case 1 -> TunerConstants.FrontRight;
- case 2 -> TunerConstants.BackLeft;
- case 3 -> TunerConstants.BackRight;
+ case 0 -> TunerFactory.INSTANCE.frontLeft();
+ case 1 -> TunerFactory.INSTANCE.frontRight();
+ case 2 -> TunerFactory.INSTANCE.backLeft();
+ case 3 -> TunerFactory.INSTANCE.backRight();
default -> throw new IllegalArgumentException("Invalid module index");
};
@@ -150,6 +148,8 @@ public ModuleIOTalonFX(int module) {
turnTalon = new TalonFX(constants.SteerMotorId, canBus);
cancoder = new CANcoder(constants.EncoderId, canBus);
+ Logger.recordOutput("Drive/EncoderOffsets/Module" + module, constants.EncoderOffset);
+
// Configure drive motor
driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
driveConfig.Slot0 =
@@ -161,19 +161,19 @@ public ModuleIOTalonFX(int module) {
.withKV(DrivebaseConstants.kDriveV)
.withKA(DrivebaseConstants.kDriveA);
driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio;
- driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DrivebaseConstants.kSlipCurrent;
- driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DrivebaseConstants.kSlipCurrent;
+ driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DrivebaseConstants.kSlipCurrentAmps;
+ driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DrivebaseConstants.kSlipCurrentAmps;
driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit;
driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
// Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing
OpenLoopRampsConfigs openRamps = new OpenLoopRampsConfigs();
- openRamps.DutyCycleOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod;
- openRamps.VoltageOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod;
- openRamps.TorqueOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod;
+ openRamps.DutyCycleOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriodSecs;
+ openRamps.VoltageOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriodSecs;
+ openRamps.TorqueOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriodSecs;
ClosedLoopRampsConfigs closedRamps = new ClosedLoopRampsConfigs();
- closedRamps.DutyCycleClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod;
- closedRamps.VoltageClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod;
- closedRamps.TorqueClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod;
+ closedRamps.DutyCycleClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriodSecs;
+ closedRamps.VoltageClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriodSecs;
+ closedRamps.TorqueClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriodSecs;
// Apply the open- and closed-loop ramp configuration for current smoothing
driveConfig.withClosedLoopRamps(closedRamps).withOpenLoopRamps(openRamps);
// Set motor inversions
@@ -219,7 +219,7 @@ public ModuleIOTalonFX(int module) {
constants.EncoderInverted
? SensorDirectionValue.Clockwise_Positive
: SensorDirectionValue.CounterClockwise_Positive;
- cancoder.getConfigurator().apply(cancoderConfig);
+ PhoenixUtil.tryUntilOk(5, () -> cancoder.getConfigurator().apply(cancoderConfig));
// Create timestamp queue
timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
@@ -310,9 +310,9 @@ public void updateInputs(ModuleIOInputs inputs) {
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];
+ inputs.odometryTimestamps = EMPTY_DOUBLE_ARRAY;
+ inputs.odometryDrivePositionsRad = EMPTY_DOUBLE_ARRAY;
+ inputs.odometryTurnPositions = EMPTY_ROTATION_ARRAY;
return;
}
@@ -353,13 +353,9 @@ public void updateInputs(ModuleIOInputs inputs) {
public void setDriveOpenLoop(double output) {
// Scale by actual battery voltage to keep full output consistent
double busVoltage = RobotController.getBatteryVoltage();
- double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double scaledOutput = output * DrivebaseConstants.kNominalVoltage / busVoltage;
- driveTalon.setControl(
- switch (m_DriveMotorClosedLoopOutput) {
- case Voltage -> voltageRequest.withOutput(scaledOutput);
- case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(scaledOutput);
- });
+ driveTalon.setControl(voltageRequest.withOutput(scaledOutput));
// Log output and battery
Logger.recordOutput("Swerve/Drive/OpenLoopOutput", scaledOutput);
@@ -374,13 +370,9 @@ public void setDriveOpenLoop(double output) {
@Override
public void setTurnOpenLoop(double output) {
double busVoltage = RobotController.getBatteryVoltage();
- double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double scaledOutput = output * DrivebaseConstants.kNominalVoltage / busVoltage;
- turnTalon.setControl(
- switch (m_SteerMotorClosedLoopOutput) {
- case Voltage -> voltageRequest.withOutput(scaledOutput);
- case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(scaledOutput);
- });
+ turnTalon.setControl(voltageRequest.withOutput(scaledOutput));
// Log output and battery
Logger.recordOutput("Swerve/Turn/OpenLoopOutput", scaledOutput);
@@ -411,7 +403,7 @@ public void setDriveVelocity(double velocityRadPerSec) {
Math.signum(velocityRotPerSec) * DrivebaseConstants.kDriveS
+ DrivebaseConstants.kDriveV * velocityRotPerSec
+ DrivebaseConstants.kDriveA * accelerationRotPerSec2;
- double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kNominalVoltage / busVoltage;
// Set the drive motor control based on CTRE LICENSED status
driveTalon.setControl(
@@ -441,8 +433,8 @@ public void setTurnPosition(Rotation2d rotation) {
double busVoltage = RobotController.getBatteryVoltage();
// Scale feedforward voltage by battery voltage
- double nominalFFVolts = DrivebaseConstants.kNominalFFVolts;
- double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage;
+ double nominalFFVolts = DrivebaseConstants.kNominalFeedforwardVolts;
+ double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kNominalVoltage / busVoltage;
turnTalon.setControl(
switch (m_SteerMotorClosedLoopOutput) {
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java
index 6226ed2f..bd618b43 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java
@@ -176,17 +176,18 @@
// driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
// driveConfig.Slot0 = constants.DriveMotorGains;
// driveConfig.ExternalFeedback.SensorToMechanismRatio = constants.DriveMotorGearRatio;
-// driveConfig.CurrentLimits.StatorCurrentLimit = DrivebaseConstants.kSlipCurrent;
+// driveConfig.CurrentLimits.StatorCurrentLimit = DrivebaseConstants.kSlipCurrentAmps;
// driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
// // Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing
// OpenLoopRampsConfigs openRamps = new OpenLoopRampsConfigs();
-// openRamps.DutyCycleOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod;
-// openRamps.VoltageOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod;
-// openRamps.TorqueOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriod;
+// openRamps.DutyCycleOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriodSecs;
+// openRamps.VoltageOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriodSecs;
+// openRamps.TorqueOpenLoopRampPeriod = DrivebaseConstants.kDriveOpenLoopRampPeriodSecs;
// ClosedLoopRampsConfigs closedRamps = new ClosedLoopRampsConfigs();
-// closedRamps.DutyCycleClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod;
-// closedRamps.VoltageClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod;
-// closedRamps.TorqueClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriod;
+// closedRamps.DutyCycleClosedLoopRampPeriod =
+// DrivebaseConstants.kDriveClosedLoopRampPeriodSecs;
+// closedRamps.VoltageClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriodSecs;
+// closedRamps.TorqueClosedLoopRampPeriod = DrivebaseConstants.kDriveClosedLoopRampPeriodSecs;
// // Apply the open- and closed-loop ramp configuration for current smoothing
// driveConfig.withClosedLoopRamps(closedRamps).withOpenLoopRamps(openRamps);
// // Set motor inversions
@@ -388,7 +389,7 @@
// public void setDriveOpenLoop(double output) {
// // Scale by actual battery voltage to keep full output consistent
// double busVoltage = RobotController.getBatteryVoltage();
-// double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage;
+// double scaledOutput = output * DrivebaseConstants.kNominalVoltage / busVoltage;
// driveTalon.setControl(
// switch (m_DriveMotorClosedLoopOutput) {
@@ -409,7 +410,7 @@
// @Override
// public void setTurnOpenLoop(double output) {
// double busVoltage = RobotController.getBatteryVoltage();
-// double scaledOutput = output * DrivebaseConstants.kOptimalVoltage / busVoltage;
+// double scaledOutput = output * DrivebaseConstants.kNominalVoltage / busVoltage;
// turnTalon.setControl(
// switch (m_SteerMotorClosedLoopOutput) {
@@ -446,7 +447,7 @@
// Math.signum(velocityRotPerSec) * DrivebaseConstants.kDriveS
// + DrivebaseConstants.kDriveV * velocityRotPerSec
// + DrivebaseConstants.kDriveA * accelerationRotPerSec2;
-// double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage;
+// double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kNominalVoltage / busVoltage;
// // Set the drive motor control based on CTRE LICENSED status
// driveTalon.setControl(
@@ -478,8 +479,8 @@
// double busVoltage = RobotController.getBatteryVoltage();
// // Scale feedforward voltage by battery voltage
-// double nominalFFVolts = DrivebaseConstants.kNominalFFVolts;
-// double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kOptimalVoltage / busVoltage;
+// double nominalFFVolts = DrivebaseConstants.kNominalFeedforwardVolts;
+// double scaledFFVolts = nominalFFVolts * DrivebaseConstants.kNominalVoltage / busVoltage;
// turnTalon.setControl(
// switch (m_SteerMotorClosedLoopOutput) {
diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java
index 75eac9d0..e19f5eec 100644
--- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java
+++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java
@@ -12,8 +12,9 @@
import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
-import frc.robot.generated.TunerConstants;
+import frc.robot.generated.TunerFactory;
import java.util.ArrayList;
import java.util.List;
import java.util.Queue;
@@ -40,7 +41,7 @@ public class PhoenixOdometryThread extends Thread {
private final List> genericQueues = new ArrayList<>();
private final List> timestampQueues = new ArrayList<>();
- private static boolean isCANFD = TunerConstants.kCANBus.isNetworkFD();
+ private static final boolean isCANFD = TunerFactory.INSTANCE.canBus().isNetworkFD();
private static PhoenixOdometryThread instance = null;
private long droppedSamples = 0;
@@ -126,7 +127,9 @@ public void run() {
if (phoenixSignals.length > 0) BaseStatusSignal.refreshAll(phoenixSignals);
}
} catch (InterruptedException e) {
- e.printStackTrace();
+ DriverStation.reportWarning("Phoenix odometry thread interrupted", e.getStackTrace());
+ Thread.currentThread().interrupt();
+ return;
} finally {
signalsLock.unlock();
}
@@ -151,7 +154,7 @@ public void run() {
if (!phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble())) droppedSamples++;
}
for (int i = 0; i < genericSignals.size(); i++) {
- genericQueues.get(i).offer(genericSignals.get(i).getAsDouble());
+ if (!genericQueues.get(i).offer(genericSignals.get(i).getAsDouble())) droppedSamples++;
}
for (int i = 0; i < timestampQueues.size(); i++) {
if (!timestampQueues.get(i).offer(timestamp)) droppedSamples++;
diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java
index 0a5d052a..f4214b10 100644
--- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java
+++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java
@@ -18,6 +18,7 @@
import java.util.Queue;
import java.util.concurrent.ArrayBlockingQueue;
import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.Logger;
/**
* Provides an interface for asynchronously reading high-frequency measurements to a set of queues.
@@ -35,6 +36,8 @@ public class SparkOdometryThread {
private static SparkOdometryThread instance = null;
private Notifier notifier = new Notifier(this::run);
+ private long droppedSamples = 0;
+ private long loopCount = 0;
public static SparkOdometryThread getInstance() {
if (instance == null) {
@@ -112,17 +115,21 @@ private void run() {
// If valid, add values to queues
if (isValid) {
for (int i = 0; i < sparkSignals.size(); i++) {
- sparkQueues.get(i).offer(sparkValues[i]);
+ if (!sparkQueues.get(i).offer(sparkValues[i])) droppedSamples++;
}
for (int i = 0; i < genericSignals.size(); i++) {
- genericQueues.get(i).offer(genericSignals.get(i).getAsDouble());
+ if (!genericQueues.get(i).offer(genericSignals.get(i).getAsDouble())) droppedSamples++;
}
for (int i = 0; i < timestampQueues.size(); i++) {
- timestampQueues.get(i).offer(timestamp);
+ if (!timestampQueues.get(i).offer(timestamp)) droppedSamples++;
}
}
} finally {
Drive.odometryLock.unlock();
}
+
+ if ((loopCount++ % (int) SwerveConstants.kOdometryFrequency) == 0) {
+ Logger.recordOutput("Drive/SparkOdomThread/DroppedSamples", droppedSamples);
+ }
}
}
diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java
index ceaff2ba..50da778b 100644
--- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java
+++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java
@@ -17,7 +17,7 @@
import edu.wpi.first.math.util.Units;
import frc.robot.Constants;
import frc.robot.Constants.CANBuses;
-import frc.robot.generated.TunerConstants;
+import frc.robot.generated.TunerFactory;
import frc.robot.subsystems.imu.ImuIO;
import frc.robot.subsystems.imu.ImuIONavX;
import frc.robot.subsystems.imu.ImuIOPigeon2;
@@ -108,86 +108,86 @@ public class SwerveConstants {
// Fill in the values from the proper source
static {
+ var tuner = TunerFactory.INSTANCE;
+
switch (Constants.getSwerveType()) {
case PHOENIX6:
kImuType = "pigeon2";
- kCoupleRatio = TunerConstants.FrontLeft.CouplingGearRatio;
- kDriveGearRatio = TunerConstants.FrontLeft.DriveMotorGearRatio;
- kSteerGearRatio = TunerConstants.FrontLeft.SteerMotorGearRatio;
+ kCoupleRatio = tuner.frontLeft().CouplingGearRatio;
+ kDriveGearRatio = tuner.frontLeft().DriveMotorGearRatio;
+ kSteerGearRatio = tuner.frontLeft().SteerMotorGearRatio;
kCANbusName = CANBuses.DRIVE;
- kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id;
- kSteerInertia = TunerConstants.FrontLeft.SteerInertia;
- kDriveInertia = TunerConstants.FrontLeft.DriveInertia;
+ kPigeonId = tuner.drivetrain().Pigeon2Id;
+ kSteerInertia = tuner.frontLeft().SteerInertia;
+ kDriveInertia = tuner.frontLeft().DriveInertia;
kSteerFrictionVoltage = 0.0;
kDriveFrictionVoltage = 0.1;
kSteerCurrentLimit = 40.0; // Example from CTRE documentation
kDriveCurrentLimit = 120.0; // Example from CTRE documentation
// Front Left
- kFLDriveMotorId = TunerConstants.FrontLeft.DriveMotorId;
- kFLSteerMotorId = TunerConstants.FrontLeft.SteerMotorId;
- kFLEncoderId = TunerConstants.FrontLeft.EncoderId;
+ kFLDriveMotorId = tuner.frontLeft().DriveMotorId;
+ kFLSteerMotorId = tuner.frontLeft().SteerMotorId;
+ kFLEncoderId = tuner.frontLeft().EncoderId;
kFLDriveCanbus = kCANbusName;
kFLSteerCanbus = kCANbusName;
kFLEncoderCanbus = kCANbusName;
kFLDriveType = "kraken";
kFLSteerType = "kraken";
kFLEncoderType = "cancoder";
- kFLEncoderOffset =
- -Units.rotationsToRadians(TunerConstants.FrontLeft.EncoderOffset) + Math.PI;
- kFLDriveInvert = TunerConstants.FrontLeft.DriveMotorInverted;
- kFLSteerInvert = TunerConstants.FrontLeft.SteerMotorInverted;
- kFLEncoderInvert = TunerConstants.FrontLeft.EncoderInverted;
- kFLXPosMeters = TunerConstants.FrontLeft.LocationX;
- kFLYPosMeters = TunerConstants.FrontLeft.LocationY;
+ kFLEncoderOffset = -Units.rotationsToRadians(tuner.frontLeft().EncoderOffset) + Math.PI;
+ kFLDriveInvert = tuner.frontLeft().DriveMotorInverted;
+ kFLSteerInvert = tuner.frontLeft().SteerMotorInverted;
+ kFLEncoderInvert = tuner.frontLeft().EncoderInverted;
+ kFLXPosMeters = tuner.frontLeft().LocationX;
+ kFLYPosMeters = tuner.frontLeft().LocationY;
// Front Right
- kFRDriveMotorId = TunerConstants.FrontRight.DriveMotorId;
- kFRSteerMotorId = TunerConstants.FrontRight.SteerMotorId;
- kFREncoderId = TunerConstants.FrontRight.EncoderId;
+ kFRDriveMotorId = tuner.frontRight().DriveMotorId;
+ kFRSteerMotorId = tuner.frontRight().SteerMotorId;
+ kFREncoderId = tuner.frontRight().EncoderId;
kFRDriveCanbus = kCANbusName;
kFRSteerCanbus = kCANbusName;
kFREncoderCanbus = kCANbusName;
kFRDriveType = "kraken";
kFRSteerType = "kraken";
kFREncoderType = "cancoder";
- kFREncoderOffset = -Units.rotationsToRadians(TunerConstants.FrontRight.EncoderOffset);
- kFRDriveInvert = TunerConstants.FrontRight.DriveMotorInverted;
- kFRSteerInvert = TunerConstants.FrontRight.SteerMotorInverted;
- kFREncoderInvert = TunerConstants.FrontRight.EncoderInverted;
- kFRXPosMeters = TunerConstants.FrontRight.LocationX;
- kFRYPosMeters = TunerConstants.FrontRight.LocationY;
+ kFREncoderOffset = -Units.rotationsToRadians(tuner.frontRight().EncoderOffset);
+ kFRDriveInvert = tuner.frontRight().DriveMotorInverted;
+ kFRSteerInvert = tuner.frontRight().SteerMotorInverted;
+ kFREncoderInvert = tuner.frontRight().EncoderInverted;
+ kFRXPosMeters = tuner.frontRight().LocationX;
+ kFRYPosMeters = tuner.frontRight().LocationY;
// Back Left
- kBLDriveMotorId = TunerConstants.BackLeft.DriveMotorId;
- kBLSteerMotorId = TunerConstants.BackLeft.SteerMotorId;
- kBLEncoderId = TunerConstants.BackLeft.EncoderId;
+ kBLDriveMotorId = tuner.backLeft().DriveMotorId;
+ kBLSteerMotorId = tuner.backLeft().SteerMotorId;
+ kBLEncoderId = tuner.backLeft().EncoderId;
kBLDriveCanbus = kCANbusName;
kBLSteerCanbus = kCANbusName;
kBLEncoderCanbus = kCANbusName;
kBLDriveType = "kraken";
kBLSteerType = "kraken";
kBLEncoderType = "cancoder";
- kBLEncoderOffset =
- -Units.rotationsToRadians(TunerConstants.BackLeft.EncoderOffset) + Math.PI;
- kBLDriveInvert = TunerConstants.BackLeft.DriveMotorInverted;
- kBLSteerInvert = TunerConstants.BackLeft.SteerMotorInverted;
- kBLEncoderInvert = TunerConstants.BackLeft.EncoderInverted;
- kBLXPosMeters = TunerConstants.BackLeft.LocationX;
- kBLYPosMeters = TunerConstants.BackLeft.LocationY;
+ kBLEncoderOffset = -Units.rotationsToRadians(tuner.backLeft().EncoderOffset) + Math.PI;
+ kBLDriveInvert = tuner.backLeft().DriveMotorInverted;
+ kBLSteerInvert = tuner.backLeft().SteerMotorInverted;
+ kBLEncoderInvert = tuner.backLeft().EncoderInverted;
+ kBLXPosMeters = tuner.backLeft().LocationX;
+ kBLYPosMeters = tuner.backLeft().LocationY;
// Back Right
- kBRDriveMotorId = TunerConstants.BackRight.DriveMotorId;
- kBRSteerMotorId = TunerConstants.BackRight.SteerMotorId;
- kBREncoderId = TunerConstants.BackRight.EncoderId;
+ kBRDriveMotorId = tuner.backRight().DriveMotorId;
+ kBRSteerMotorId = tuner.backRight().SteerMotorId;
+ kBREncoderId = tuner.backRight().EncoderId;
kBRDriveCanbus = kCANbusName;
kBRSteerCanbus = kCANbusName;
kBREncoderCanbus = kCANbusName;
kBRDriveType = "kraken";
kBRSteerType = "kraken";
kBREncoderType = "cancoder";
- kBREncoderOffset = -Units.rotationsToRadians(TunerConstants.BackRight.EncoderOffset);
- kBRDriveInvert = TunerConstants.BackRight.DriveMotorInverted;
- kBRSteerInvert = TunerConstants.BackRight.SteerMotorInverted;
- kBREncoderInvert = TunerConstants.BackRight.EncoderInverted;
- kBRXPosMeters = TunerConstants.BackRight.LocationX;
- kBRYPosMeters = TunerConstants.BackRight.LocationY;
+ kBREncoderOffset = -Units.rotationsToRadians(tuner.backRight().EncoderOffset);
+ kBRDriveInvert = tuner.backRight().DriveMotorInverted;
+ kBRSteerInvert = tuner.backRight().SteerMotorInverted;
+ kBREncoderInvert = tuner.backRight().EncoderInverted;
+ kBRXPosMeters = tuner.backRight().LocationX;
+ kBRYPosMeters = tuner.backRight().LocationY;
break;
case YAGSL:
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java
index 80406e50..35d446b2 100644
--- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java
@@ -9,10 +9,14 @@
package frc.robot.subsystems.flywheel_example;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Volts;
import static frc.robot.Constants.FlywheelConstants.*;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;
@@ -23,7 +27,8 @@
public class Flywheel extends RBSISubsystem {
private final FlywheelIO io;
private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged();
- private final SysIdRoutine sysId;
+ private final SysIdRoutine voltageSysId;
+ private final SysIdRoutine dutyCycleSysId;
/** Creates a new Flywheel. */
public Flywheel(FlywheelIO io) {
@@ -34,23 +39,17 @@ public Flywheel(FlywheelIO io) {
switch (Constants.getMode()) {
case REAL:
case REPLAY:
- io.configureGains(kPreal, 0.0, kDreal, kSreal, kVreal, kAreal);
+ io.configureGains(kRealP, 0.0, kRealD, kRealS, kRealV, kRealA);
break;
case SIM:
default:
- io.configureGains(kPsim, 0.0, kDsim, kSsim, kVsim, kAsim);
+ io.configureGains(kSimP, 0.0, kSimD, kSimS, kSimV, kSimA);
break;
}
- // Configure SysId
- sysId =
- new SysIdRoutine(
- new SysIdRoutine.Config(
- null,
- null,
- null,
- (state) -> Logger.recordOutput("Flywheel/SysIdState", state.toString())),
- new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this));
+ // Configure SysId routines. The voltage routine is the preferred source for kS/kV/kA fits.
+ voltageSysId = createSysIdRoutine("FlywheelVoltage", this::runVolts);
+ dutyCycleSysId = createSysIdRoutine("FlywheelDutyCycle", this::runDutyCycleForSysIdVolts);
}
/** Periodic function -- inherits timing logic from RBSISubsystem */
@@ -65,6 +64,17 @@ public void runVolts(double volts) {
io.setVoltage(volts);
}
+ /**
+ * Run open loop using duty cycle while treating the SysId output as requested motor voltage.
+ *
+ * This is useful for comparing vendor duty-cycle behavior against direct voltage control. Use
+ * the logged applied voltage, not the requested duty cycle, when fitting SysId data.
+ */
+ public void runDutyCycleForSysIdVolts(double volts) {
+ double batteryVoltage = RobotController.getBatteryVoltage();
+ io.setPercent(batteryVoltage > 0.0 ? MathUtil.clamp(volts / batteryVoltage, -1.0, 1.0) : 0.0);
+ }
+
/** Run closed loop at the specified velocity. */
public void runVelocity(double velocityRPM) {
var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM);
@@ -74,6 +84,16 @@ public void runVelocity(double velocityRPM) {
Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM);
}
+ /**
+ * Run profiled/smoothed closed loop at the specified velocity, when supported by the IO layer.
+ */
+ public void runVelocityProfiled(double velocityRPM) {
+ var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM);
+ io.setVelocityProfiled(velocityRadPerSec);
+
+ Logger.recordOutput("Flywheel/ProfiledSetpointRPM", velocityRPM);
+ }
+
/** Stops the flywheel. */
public void stop() {
io.stop();
@@ -81,12 +101,40 @@ public void stop() {
/** Returns a command to run a quasistatic test in the specified direction. */
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
- return sysId.quasistatic(direction);
+ return sysIdVoltageQuasistatic(direction);
}
/** Returns a command to run a dynamic test in the specified direction. */
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
- return sysId.dynamic(direction);
+ return sysIdVoltageDynamic(direction);
+ }
+
+ /** Returns a command to run a direct-voltage quasistatic test. */
+ public Command sysIdVoltageQuasistatic(SysIdRoutine.Direction direction) {
+ return voltageSysId
+ .quasistatic(direction)
+ .withName("Flywheel SysId Voltage " + directionLabel(direction) + " Quasistatic");
+ }
+
+ /** Returns a command to run a direct-voltage dynamic test. */
+ public Command sysIdVoltageDynamic(SysIdRoutine.Direction direction) {
+ return voltageSysId
+ .dynamic(direction)
+ .withName("Flywheel SysId Voltage " + directionLabel(direction) + " Dynamic");
+ }
+
+ /** Returns a command to run a duty-cycle quasistatic test. */
+ public Command sysIdDutyCycleQuasistatic(SysIdRoutine.Direction direction) {
+ return dutyCycleSysId
+ .quasistatic(direction)
+ .withName("Flywheel SysId Duty Cycle " + directionLabel(direction) + " Quasistatic");
+ }
+
+ /** Returns a command to run a duty-cycle dynamic test. */
+ public Command sysIdDutyCycleDynamic(SysIdRoutine.Direction direction) {
+ return dutyCycleSysId
+ .dynamic(direction)
+ .withName("Flywheel SysId Duty Cycle " + directionLabel(direction) + " Dynamic");
}
/** Returns the current velocity in RPM. */
@@ -104,4 +152,25 @@ public double getCharacterizationVelocity() {
public int[] getPowerPorts() {
return io.getPowerPorts();
}
+
+ private SysIdRoutine createSysIdRoutine(String name, java.util.function.DoubleConsumer output) {
+ return new SysIdRoutine(
+ new SysIdRoutine.Config(
+ Volts.of(kSysIdQuasistaticRampRateVoltsPerSec).per(Second),
+ Volts.of(kSysIdDynamicStepVoltageVolts),
+ Seconds.of(kSysIdTimeoutSecs),
+ (state) -> recordSysIdState(name, state)),
+ new SysIdRoutine.Mechanism(
+ (voltage) -> output.accept(voltage.in(Volts)), null, this, name));
+ }
+
+ private void recordSysIdState(String routineName, SysIdRoutine.State state) {
+ Logger.recordOutput("SysIdTestState", state.toString());
+ Logger.recordOutput("Flywheel/SysIdRoutine", routineName);
+ Logger.recordOutput("Flywheel/SysIdState", state.toString());
+ }
+
+ private static String directionLabel(SysIdRoutine.Direction direction) {
+ return direction == SysIdRoutine.Direction.kForward ? "Forward" : "Reverse";
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java
index 47fa8748..ac4f7cf5 100644
--- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java
@@ -28,6 +28,11 @@ public default void updateInputs(FlywheelIOInputs inputs) {}
/** Run closed loop at the specified velocity. */
public default void setVelocity(double velocityRadPerSec) {}
+ /** Run closed loop at the specified velocity using a profiled/smoothed velocity request. */
+ public default void setVelocityProfiled(double velocityRadPerSec) {
+ setVelocity(velocityRadPerSec);
+ }
+
/** Set gain constants */
public default void configureGains(double kP, double kI, double kD, double kS, double kV) {}
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java
index a8e8c20f..34ce798d 100644
--- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java
@@ -16,21 +16,18 @@
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
-import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import frc.robot.Constants;
+import frc.robot.Constants.FlywheelConstants;
public class FlywheelIOSim implements FlywheelIO {
- // Reduction between motors and encoder, as output over input. If the flywheel
- // spins slower than the motors, this number should be greater than one.
- private static final double kFlywheelGearing = 1.0;
-
- // 1/2 MR^2
- private static final double kFlywheelMomentOfInertia =
- 0.5 * Units.lbsToKilograms(1.5) * Math.pow(Units.inchesToMeters(4), 2);
-
private final DCMotor m_gearbox = DCMotor.getNEO(1);
private final LinearSystem m_plant =
- LinearSystemId.createFlywheelSystem(m_gearbox, kFlywheelGearing, kFlywheelMomentOfInertia);
+ LinearSystemId.createFlywheelSystem(
+ m_gearbox,
+ FlywheelConstants.kSimGearing,
+ FlywheelConstants.kSimMomentOfInertiaKgMetersSq);
private final FlywheelSim sim = new FlywheelSim(m_plant, m_gearbox);
private PIDController pid = new PIDController(0.0, 0.0, 0.0);
@@ -44,11 +41,14 @@ public class FlywheelIOSim implements FlywheelIO {
public void updateInputs(FlywheelIOInputs inputs) {
if (closedLoop) {
appliedVolts =
- MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0);
+ MathUtil.clamp(
+ pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts,
+ -FlywheelConstants.kMaxVoltage,
+ FlywheelConstants.kMaxVoltage);
sim.setInputVoltage(appliedVolts);
}
- sim.update(0.02);
+ sim.update(Constants.kLoopPeriodSecs);
inputs.positionRad = 0.0;
inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec();
@@ -63,6 +63,11 @@ public void setVoltage(double volts) {
sim.setInputVoltage(volts);
}
+ @Override
+ public void setPercent(double percent) {
+ setVoltage(percent * RobotController.getBatteryVoltage());
+ }
+
@Override
public void setVelocity(double velocityRadPerSec) {
closedLoop = true;
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java
index ee89859e..e847f8cf 100644
--- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java
@@ -29,7 +29,7 @@
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.util.Units;
import frc.robot.Constants;
@@ -55,55 +55,70 @@ public class FlywheelIOSpark implements FlywheelIO {
public final int[] powerPorts = {
FLYWHEEL_LEADER.getPowerPort(), FLYWHEEL_FOLLOWER.getPowerPort()
};
- private final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kSreal, kVreal, kAreal);
+ private final SparkMaxConfig leaderConfig = new SparkMaxConfig();
+ private SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kRealS, kRealV, kRealA);
+
+ @Override
+ public int[] powerPorts() {
+ return powerPorts;
+ }
public FlywheelIOSpark() {
// Configure leader motor
- var leaderConfig = new SparkFlexConfig();
leaderConfig
.idleMode(
- switch (kFlywheelIdleMode) {
+ switch (kIdleMode) {
case COAST -> IdleMode.kCoast;
case BRAKE -> IdleMode.kBrake;
})
.smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit)
- .voltageCompensation(DrivebaseConstants.kOptimalVoltage);
+ .voltageCompensation(DrivebaseConstants.kNominalVoltage);
leaderConfig.encoder.uvwMeasurementPeriod(10).uvwAverageDepth(2);
leaderConfig
.closedLoop
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
- .pid(kPreal, 0.0, kDreal)
+ .pid(kRealP, 0.0, kRealD)
.feedForward
- .kS(kSreal)
- .kV(kVreal)
- .kA(kAreal);
+ .kS(kRealS)
+ .kV(kRealV)
+ .kA(kRealA);
leaderConfig
.signals
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency))
.primaryEncoderVelocityAlwaysOn(true)
- .primaryEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .busVoltagePeriodMs((int) (Constants.loopPeriodSecs * 1000.))
- .outputCurrentPeriodMs((int) (Constants.loopPeriodSecs * 1000.));
+ .primaryEncoderVelocityPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .appliedOutputPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .busVoltagePeriodMs((int) (Constants.kLoopPeriodSecs * 1000.))
+ .outputCurrentPeriodMs((int) (Constants.kLoopPeriodSecs * 1000.));
leaderConfig
- .openLoopRampRate(DrivebaseConstants.kDriveOpenLoopRampPeriod)
- .closedLoopRampRate(DrivebaseConstants.kDriveClosedLoopRampPeriod);
+ .openLoopRampRate(DrivebaseConstants.kDriveOpenLoopRampPeriodSecs)
+ .closedLoopRampRate(DrivebaseConstants.kDriveClosedLoopRampPeriodSecs);
SparkUtil.tryUntilOk(
leader,
5,
() ->
leader.configure(
leaderConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
+
+ var followerConfig = new SparkMaxConfig();
+ followerConfig.follow(leader);
+ SparkUtil.tryUntilOk(
+ follower,
+ 5,
+ () ->
+ follower.configure(
+ followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters));
+
SparkUtil.tryUntilOk(leader, 5, () -> encoder.setPosition(0.0));
}
@Override
public void updateInputs(FlywheelIOInputs inputs) {
- inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / kFlywheelGearRatio);
+ inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / kGearRatio);
inputs.velocityRadPerSec =
- Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / kFlywheelGearRatio);
+ Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / kGearRatio);
inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage();
inputs.currentAmps = new double[] {leader.getOutputCurrent(), follower.getOutputCurrent()};
@@ -120,11 +135,16 @@ public void setVoltage(double volts) {
leader.setVoltage(volts);
}
+ @Override
+ public void setPercent(double percent) {
+ leader.set(percent);
+ }
+
@Override
public void setVelocity(double velocityRadPerSec) {
double ffVolts = ff.calculate(velocityRadPerSec);
pid.setSetpoint(
- Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * kFlywheelGearRatio,
+ Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * kGearRatio,
ControlType.kVelocity,
ClosedLoopSlot.kSlot0,
ffVolts,
@@ -139,23 +159,23 @@ public void stop() {
/**
* Configure the closed-loop control gains
*
- * TODO: This functionality is no longer supported by the REVLib SparkClosedLoopController
- * class. In order to keep control of the flywheel's underlying funtionality, shift everything to
- * SmartMotion control.
+ *
REVLib 2026 applies closed-loop gains through the Spark configuration object rather than
+ * direct setters on {@link SparkClosedLoopController}.
*/
@Override
public void configureGains(double kP, double kI, double kD, double kS, double kV) {
- // pid.setP(kP, 0);
- // pid.setI(kI, 0);
- // pid.setD(kD, 0);
- // pid.setFF(0, 0);
+ configureGains(kP, kI, kD, kS, kV, 0.0);
}
@Override
public void configureGains(double kP, double kI, double kD, double kS, double kV, double kA) {
- // pid.setP(kP, 0);
- // pid.setI(kI, 0);
- // pid.setD(kD, 0);
- // pid.setFF(0, 0);
+ ff = new SimpleMotorFeedforward(kS, kV, kA);
+ leaderConfig.closedLoop.pid(kP, kI, kD).feedForward.kS(kS).kV(kV).kA(kA);
+ SparkUtil.tryUntilOk(
+ leader,
+ 5,
+ () ->
+ leader.configure(
+ leaderConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters));
}
}
diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java
index 1b964f5c..a9bb0f91 100644
--- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java
@@ -17,10 +17,11 @@
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.MotorAlignmentValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
@@ -46,6 +47,11 @@ public class FlywheelIOTalonFX implements FlywheelIO {
FLYWHEEL_LEADER.getPowerPort(), FLYWHEEL_FOLLOWER.getPowerPort()
};
+ @Override
+ public int[] powerPorts() {
+ return powerPorts;
+ }
+
private final StatusSignal leaderPosition = leader.getPosition();
private final StatusSignal leaderVelocity = leader.getVelocity();
private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage();
@@ -54,31 +60,36 @@ public class FlywheelIOTalonFX implements FlywheelIO {
private final TalonFXConfiguration config = new TalonFXConfiguration();
private final boolean isCTREPro = Constants.getPhoenixPro() == CTREPro.LICENSED;
+ private final VoltageOut voltageRequest = new VoltageOut(0);
+ private final DutyCycleOut dutyCycleRequest = new DutyCycleOut(0);
+ private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0);
+ private final MotionMagicVelocityVoltage motionMagicVelocityRequest =
+ new MotionMagicVelocityVoltage(0);
public FlywheelIOTalonFX() {
- config.CurrentLimits.SupplyCurrentLimit = PowerConstants.kMotorPortMaxCurrent;
+ config.CurrentLimits.SupplyCurrentLimit = PowerConstants.kMotorPortMaxCurrentAmps;
config.CurrentLimits.SupplyCurrentLimitEnable = true;
config.MotorOutput.NeutralMode =
- switch (kFlywheelIdleMode) {
+ switch (kIdleMode) {
case COAST -> NeutralModeValue.Coast;
case BRAKE -> NeutralModeValue.Brake;
};
// Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing
OpenLoopRampsConfigs openRamps = new OpenLoopRampsConfigs();
- openRamps.DutyCycleOpenLoopRampPeriod = kFlywheelOpenLoopRampPeriod;
- openRamps.VoltageOpenLoopRampPeriod = kFlywheelOpenLoopRampPeriod;
- openRamps.TorqueOpenLoopRampPeriod = kFlywheelOpenLoopRampPeriod;
+ openRamps.DutyCycleOpenLoopRampPeriod = kOpenLoopRampPeriodSecs;
+ openRamps.VoltageOpenLoopRampPeriod = kOpenLoopRampPeriodSecs;
+ openRamps.TorqueOpenLoopRampPeriod = kOpenLoopRampPeriodSecs;
ClosedLoopRampsConfigs closedRamps = new ClosedLoopRampsConfigs();
- closedRamps.DutyCycleClosedLoopRampPeriod = kFlywheelClosedLoopRampPeriod;
- closedRamps.VoltageClosedLoopRampPeriod = kFlywheelClosedLoopRampPeriod;
- closedRamps.TorqueClosedLoopRampPeriod = kFlywheelClosedLoopRampPeriod;
+ closedRamps.DutyCycleClosedLoopRampPeriod = kClosedLoopRampPeriodSecs;
+ closedRamps.VoltageClosedLoopRampPeriod = kClosedLoopRampPeriodSecs;
+ closedRamps.TorqueClosedLoopRampPeriod = kClosedLoopRampPeriodSecs;
// Apply the open- and closed-loop ramp configuration for current smoothing
config.withClosedLoopRamps(closedRamps).withOpenLoopRamps(openRamps);
// set Motion Magic Velocity settings
var motionMagicConfigs = config.MotionMagic;
motionMagicConfigs.MotionMagicAcceleration =
- 400; // Target acceleration of 400 rps/s (0.25 seconds to max)
- motionMagicConfigs.MotionMagicJerk = 4000; // Target jerk of 4000 rps/s/s (0.1 seconds)
+ kMotionMagicAccelerationRotPerSecSq; // Target acceleration in rotations/s/s
+ motionMagicConfigs.MotionMagicJerk = kMotionMagicJerkRotPerSecCubed; // rotations/s/s/s
// Apply the configurations to the flywheel motors
PhoenixUtil.tryUntilOk(5, () -> leader.getConfigurator().apply(config, 0.25));
@@ -96,10 +107,9 @@ public FlywheelIOTalonFX() {
public void updateInputs(FlywheelIOInputs inputs) {
BaseStatusSignal.refreshAll(
leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent);
- inputs.positionRad =
- Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / kFlywheelGearRatio;
+ inputs.positionRad = Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / kGearRatio;
inputs.velocityRadPerSec =
- Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / kFlywheelGearRatio;
+ Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / kGearRatio;
inputs.appliedVolts = leaderAppliedVolts.getValueAsDouble();
inputs.currentAmps =
new double[] {leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()};
@@ -107,25 +117,28 @@ public void updateInputs(FlywheelIOInputs inputs) {
@Override
public void setVoltage(double volts) {
- final MotionMagicVoltage m_request = new MotionMagicVoltage(volts);
- m_request.withEnableFOC(isCTREPro);
- leader.setControl(m_request);
+ leader.setControl(voltageRequest.withOutput(volts).withEnableFOC(isCTREPro));
}
@Override
public void setVelocity(double velocityRadPerSec) {
- // create a Motion Magic Velocity request, voltage output
- final MotionMagicVelocityVoltage m_request = new MotionMagicVelocityVoltage(0);
- m_request.withEnableFOC(isCTREPro);
- leader.setControl(m_request.withVelocity(Units.radiansToRotations(velocityRadPerSec)));
+ leader.setControl(
+ velocityVoltageRequest
+ .withVelocity(Units.radiansToRotations(velocityRadPerSec) * kGearRatio)
+ .withEnableFOC(isCTREPro));
+ }
+
+ @Override
+ public void setVelocityProfiled(double velocityRadPerSec) {
+ leader.setControl(
+ motionMagicVelocityRequest
+ .withVelocity(Units.radiansToRotations(velocityRadPerSec) * kGearRatio)
+ .withEnableFOC(isCTREPro));
}
@Override
public void setPercent(double percent) {
- // create a Motion Magic DutyCycle request, voltage output
- final MotionMagicDutyCycle m_request = new MotionMagicDutyCycle(percent);
- m_request.withEnableFOC(isCTREPro);
- leader.setControl(m_request);
+ leader.setControl(dutyCycleRequest.withOutput(percent).withEnableFOC(isCTREPro));
}
@Override
diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java
index 854f7182..c163a2e0 100644
--- a/src/main/java/frc/robot/subsystems/imu/Imu.java
+++ b/src/main/java/frc/robot/subsystems/imu/Imu.java
@@ -54,6 +54,10 @@ public void rbsiPeriodic() {
io.updateInputs(inputs);
}
+ public boolean isConnected() {
+ return inputs.connected;
+ }
+
/**
* Get the inputs objects
*
diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java
index 8ee521e4..1b2bea5f 100644
--- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java
+++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java
@@ -26,6 +26,7 @@
* odometry samples.
*/
public interface ImuIO extends RBSIIO {
+ double[] EMPTY_DOUBLE_ARRAY = new double[0];
@AutoLog
class ImuIOInputs {
@@ -44,9 +45,9 @@ class ImuIOInputs {
// Time spent in the IO update call (seconds)
public double latencySeconds = 0.0;
// Odometry samples (timestamps in seconds)
- public double[] odometryYawTimestamps = new double[] {};
+ public double[] odometryYawTimestamps = EMPTY_DOUBLE_ARRAY;
// Odometry samples (yaw positions in radians)
- public double[] odometryYawPositionsRad = new double[] {};
+ public double[] odometryYawPositionsRad = EMPTY_DOUBLE_ARRAY;
}
/** Update the current IMU readings into `inputs` */
diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java
index 97d47100..08a44b1e 100644
--- a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java
+++ b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java
@@ -88,9 +88,9 @@ public void updateInputs(ImuIOInputs inputs) {
// World linear accel (NavX returns "g" typically); convert to m/s/s
inputs.linearAccel =
new Translation3d(
- navx.getWorldLinearAccelX() * Constants.G_TO_MPS2,
- navx.getWorldLinearAccelY() * Constants.G_TO_MPS2,
- navx.getWorldLinearAccelZ() * Constants.G_TO_MPS2);
+ navx.getWorldLinearAccelX() * Constants.kGravityMetersPerSecSq,
+ navx.getWorldLinearAccelY() * Constants.kGravityMetersPerSecSq,
+ navx.getWorldLinearAccelZ() * Constants.kGravityMetersPerSecSq);
// Jerk computed as (delta accel) / dt
if (prevTimestampNs != 0L) {
@@ -116,8 +116,8 @@ public void updateInputs(ImuIOInputs inputs) {
inputs.odometryYawPositionsRad = yawOut;
} else {
// ...otherwise return empty arrays
- inputs.odometryYawTimestamps = new double[] {};
- inputs.odometryYawPositionsRad = new double[] {};
+ inputs.odometryYawTimestamps = EMPTY_DOUBLE_ARRAY;
+ inputs.odometryYawPositionsRad = EMPTY_DOUBLE_ARRAY;
}
// Compute how long this took in seconds
diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java
index a1f708b0..7b203716 100644
--- a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java
@@ -97,9 +97,9 @@ public void updateInputs(ImuIOInputs inputs) {
// Accel: Phoenix returns "g" for these signals; convert to m/s/s
inputs.linearAccel =
new Translation3d(
- accelX.getValueAsDouble() * Constants.G_TO_MPS2,
- accelY.getValueAsDouble() * Constants.G_TO_MPS2,
- accelZ.getValueAsDouble() * Constants.G_TO_MPS2);
+ accelX.getValueAsDouble() * Constants.kGravityMetersPerSecSq,
+ accelY.getValueAsDouble() * Constants.kGravityMetersPerSecSq,
+ accelZ.getValueAsDouble() * Constants.kGravityMetersPerSecSq);
// Jerk computed as (delta accel) / dt
if (prevTimestampNs != 0L) {
@@ -126,8 +126,8 @@ public void updateInputs(ImuIOInputs inputs) {
inputs.odometryYawPositionsRad = yawOut;
} else {
// ...otherwise return empty arrays
- inputs.odometryYawTimestamps = new double[] {};
- inputs.odometryYawPositionsRad = new double[] {};
+ inputs.odometryYawTimestamps = EMPTY_DOUBLE_ARRAY;
+ inputs.odometryYawPositionsRad = EMPTY_DOUBLE_ARRAY;
}
// Compute how long this took in seconds
diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java
index 1bf04a7d..87321a75 100644
--- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java
+++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java
@@ -93,6 +93,7 @@ public void updateInputs(ImuIOInputs inputs) {
inputs.odometryYawTimestamps = tsOut;
inputs.odometryYawPositionsRad = yawOut;
+ clearOdomSamples();
// SIM logging
Logger.recordOutput("IMU/YawRad", yawRad);
@@ -109,6 +110,7 @@ public void updateInputs(ImuIOInputs inputs) {
public void zeroYawRad(double yawRad) {
this.yawRad = yawRad;
this.yawRateRadPerSec = 0.0;
+ clearOdomSamples();
}
private void pushOdomSample(double timestampSec, double yawRad) {
@@ -120,4 +122,9 @@ private void pushOdomSample(double timestampSec, double yawRad) {
if (odomSize < ODOM_CAP) odomSize++;
}
+
+ private void clearOdomSamples() {
+ odomSize = 0;
+ odomHead = 0;
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
index fda7b4b5..4d2b4ba7 100644
--- a/src/main/java/frc/robot/subsystems/vision/Vision.java
+++ b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -28,6 +28,7 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
+import frc.robot.Constants;
import frc.robot.Constants.Cameras;
import frc.robot.FieldConstants;
import frc.robot.subsystems.drive.Drive;
@@ -37,6 +38,7 @@
import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.Arrays;
+import java.util.LinkedHashSet;
import java.util.Optional;
import java.util.OptionalDouble;
import java.util.Set;
@@ -87,6 +89,8 @@ public interface PoseMeasurementConsumer {
// Variance minimum for fusing poses to prevent divide-by-zero explosions
private static final double kMinVariance = 1e-12;
+ private static final Pose3d[] kEmptyTagPoseArray = new Pose3d[0];
+ private static final int[] kEmptyTagIdArray = new int[0];
// Last smoothed and fused poses -- used for debugging
private Pose2d lastFusedPose = new Pose2d();
@@ -94,6 +98,8 @@ public interface PoseMeasurementConsumer {
private double lastFusedTs = Double.NaN;
private boolean lastFusedValid = false;
private boolean lastSmoothedValid = false;
+ private final LinkedHashSet tagIdsSeenThisLoop = new LinkedHashSet<>();
+ private final ArrayList perCamAccepted = new ArrayList<>();
/** Constructor */
public Vision(Drive drive, PoseMeasurementConsumer consumer, VisionIO... io) {
@@ -131,14 +137,16 @@ protected int getPeriodPriority() {
@Override
public void rbsiPeriodic() {
- // Debugging values
boolean hasAcceptedThisLoop = false;
boolean hasFusedThisLoop = false;
boolean hasSmoothedThisLoop = false;
+ final boolean tuningMode = Constants.isTuningMode();
try {
lastAlignDbg.reset();
+ tagIdsSeenThisLoop.clear();
+ perCamAccepted.clear();
// Pose reset gate (clears smoothing state, resets per-cam monotonic gates)
long epoch = drive.getPoseResetEpoch();
if (epoch != lastSeenPoseResetEpoch) {
@@ -156,15 +164,14 @@ public void rbsiPeriodic() {
}
// Always-on “health” debug -- may consider removing this
- Logger.recordOutput("Vision/Debug/ioLength", io.length);
int totalObs = 0;
for (int i = 0; i < io.length; i++) {
totalObs += (inputs[i].poseObservations != null) ? inputs[i].poseObservations.length : 0;
}
- Logger.recordOutput("Vision/Debug/totalObsThisLoop", totalObs);
-
- // Choose best observation per camera for THIS loop
- final ArrayList perCamAccepted = new ArrayList<>(io.length);
+ if (tuningMode) {
+ Logger.recordOutput("Vision/Debug/ioLength", io.length);
+ Logger.recordOutput("Vision/Debug/totalObsThisLoop", totalObs);
+ }
for (int cam = 0; cam < io.length; cam++) {
@@ -191,8 +198,17 @@ public void rbsiPeriodic() {
for (var obs : obsArr) {
seen++;
+ int[] tagIds = obs.usedTagIds();
+ if (tagIds != null) {
+ for (int tagId : tagIds) {
+ tagIdsSeenThisLoop.add(tagId);
+ }
+ }
+
GateResult gate = passesScrutiny(cam, obs);
- Logger.recordOutput("Vision/Camera" + cam + "/GateFail", gate.reason);
+ if (tuningMode) {
+ Logger.recordOutput("Vision/Camera" + cam + "/GateFail", gate.reason);
+ }
if (!gate.accepted) {
rejected++;
continue;
@@ -236,7 +252,9 @@ public void rbsiPeriodic() {
Logger.recordOutput("Vision/Camera" + cam + "/ObsRejected", rejected);
}
- Logger.recordOutput("Vision/Debug/perCamAcceptedSize", perCamAccepted.size());
+ if (tuningMode) {
+ Logger.recordOutput("Vision/Debug/perCamAcceptedSize", perCamAccepted.size());
+ }
if (perCamAccepted.isEmpty()) {
// No new vision accepted this loop; we still log cached outputs below (in finally).
@@ -246,8 +264,13 @@ public void rbsiPeriodic() {
// =====
// Fuse all accepted cams at the newest timestamp among them
- final double tFusion =
- perCamAccepted.stream().mapToDouble(e -> e.timestampSeconds()).max().orElse(Double.NaN);
+ double tFusion = Double.NaN;
+ for (int i = 0; i < perCamAccepted.size(); i++) {
+ final double timestamp = perCamAccepted.get(i).timestampSeconds();
+ if (!Double.isFinite(tFusion) || timestamp > tFusion) {
+ tFusion = timestamp;
+ }
+ }
if (!Double.isFinite(tFusion)) return;
final TimedPose fused = fuseAtTime(perCamAccepted, tFusion);
@@ -285,6 +308,28 @@ public void rbsiPeriodic() {
Logger.recordOutput("Vision/HasAcceptedThisLoop", hasAcceptedThisLoop);
Logger.recordOutput("Vision/HasFusedThisLoop", hasFusedThisLoop);
Logger.recordOutput("Vision/HasSmoothedThisLoop", hasSmoothedThisLoop);
+
+ Logger.recordOutput("Vision/TagCountThisLoop", tagIdsSeenThisLoop.size());
+ if (tagIdsSeenThisLoop.isEmpty()) {
+ Logger.recordOutput("Vision/TagsSeenThisLoop", kEmptyTagPoseArray);
+ Logger.recordOutput("Vision/TagIdsSeenThisLoop", kEmptyTagIdArray);
+ } else {
+ final Pose3d[] tagsSeen = new Pose3d[tagIdsSeenThisLoop.size()];
+ final int[] tagIdsSeen = new int[tagIdsSeenThisLoop.size()];
+ int tagCount = 0;
+ for (int tagId : tagIdsSeenThisLoop) {
+ tagIdsSeen[tagCount] = tagId;
+ Optional tagPose = FieldConstants.aprilTagLayout.getTagPose(tagId);
+ if (tagPose.isPresent()) {
+ tagsSeen[tagCount] = tagPose.get();
+ tagCount++;
+ }
+ }
+ Logger.recordOutput(
+ "Vision/TagsSeenThisLoop",
+ tagCount == tagsSeen.length ? tagsSeen : Arrays.copyOf(tagsSeen, tagCount));
+ Logger.recordOutput("Vision/TagIdsSeenThisLoop", tagIdsSeen);
+ }
}
}
@@ -377,11 +422,11 @@ private GateResult passesScrutiny(int cam, VisionIO.PoseObservation obs) {
if (obs.tagCount() <= 0) return new GateResult(false, "no tags");
// Single-tag ambiguity gate
- if (obs.tagCount() == 1 && obs.ambiguity() > maxAmbiguity)
+ if (obs.tagCount() == 1 && obs.ambiguity() > kMaxAmbiguity)
return new GateResult(false, "highly ambiguous");
// Z sanity
- if (Math.abs(obs.pose().getZ()) > maxZError) return new GateResult(false, "z not sane");
+ if (Math.abs(obs.pose().getZ()) > kMaxZErrorMeters) return new GateResult(false, "z not sane");
// Field bounds
Pose3d p = obs.pose();
@@ -429,13 +474,13 @@ private BuiltEstimate buildEstimate(int cam, VisionIO.PoseObservation obs) {
// Camera uncertainty factor
final double camFactor = (cam < camConfigs.length) ? camConfigs[cam].stdDevFactor() : 1.0;
- double linearStdDev = linearStdDevBaseline * camFactor * distFactor;
- double angularStdDev = angularStdDevBaseline * camFactor * distFactor;
+ double linearStdDev = kLinearStdDevBaseline * camFactor * distFactor;
+ double angularStdDev = kAngularStdDevBaseline * camFactor * distFactor;
// MegaTag2 bonus if applicable
if (obs.type() == PoseObservationType.MEGATAG_2) {
- linearStdDev *= linearStdDevMegatag2Factor;
- angularStdDev *= angularStdDevMegatag2Factor;
+ linearStdDev *= kLinearStdDevMegatag2Factor;
+ angularStdDev *= kAngularStdDevMegatag2Factor;
}
// Trusted tag blending
@@ -461,16 +506,17 @@ private BuiltEstimate buildEstimate(int cam, VisionIO.PoseObservation obs) {
linearStdDev *= trustScale;
angularStdDev *= trustScale;
- linearStdDev = Math.max(linearStdDev, linearStdDevBaseline);
- angularStdDev = Math.max(angularStdDev, angularStdDevBaseline);
+ linearStdDev = Math.max(linearStdDev, kLinearStdDevBaseline);
+ angularStdDev = Math.max(angularStdDev, kAngularStdDevBaseline);
// Output logs for tuning
- Logger.recordOutput("Vision/Camera" + cam + "/InjectedFracTrusted", fracTrusted);
-
- Logger.recordOutput("Vision/Camera" + cam + "/Dbg_linearStdDev", linearStdDev);
- Logger.recordOutput("Vision/Camera" + cam + "/Dbg_angularStdDev", angularStdDev);
- Logger.recordOutput("Vision/Camera" + cam + "/Dbg_avgDist", avgDist);
- Logger.recordOutput("Vision/Camera" + cam + "/Dbg_tagCount", obs.tagCount());
+ if (Constants.isTuningMode()) {
+ Logger.recordOutput("Vision/Camera" + cam + "/InjectedFracTrusted", fracTrusted);
+ Logger.recordOutput("Vision/Camera" + cam + "/Dbg_linearStdDev", linearStdDev);
+ Logger.recordOutput("Vision/Camera" + cam + "/Dbg_angularStdDev", angularStdDev);
+ Logger.recordOutput("Vision/Camera" + cam + "/Dbg_avgDist", avgDist);
+ Logger.recordOutput("Vision/Camera" + cam + "/Dbg_tagCount", obs.tagCount());
+ }
return new BuiltEstimate(
new TimedPose(
@@ -639,24 +685,30 @@ private TimedPose inverseVarianceFuse(ArrayList alignedAtTF, double t
sumSin += rot.getSin() * wth;
}
- // If everything got skipped; return null
- if (sumWx <= 0.0 || sumWy <= 0.0 || sumWth <= 0.0) return null;
+ // If every translational measurement got skipped, there is nothing useful to inject.
+ if (sumWx <= 0.0 || sumWy <= 0.0) return null;
// Construct the fused translation
final Translation2d fusedTranslation = new Translation2d(sumX / sumWx, sumY / sumWy);
- // Rotation2d(cos, sin) will normalize internally; if both are ~0, fall back to zero.
+ // Rotation2d(cos, sin) will normalize internally. If all accepted observations have infinite
+ // angular uncertainty (e.g. MegaTag2), retain the newest aligned rotation and report infinite
+ // theta uncertainty so the downstream estimator ignores heading correction.
+ final boolean hasAngularWeight = sumWth > 0.0;
final Rotation2d fusedRotation =
- (Math.abs(sumCos) < 1e-12 && Math.abs(sumSin) < 1e-12)
- ? Rotation2d.kZero
- : new Rotation2d(sumCos, sumSin);
+ hasAngularWeight
+ ? new Rotation2d(sumCos, sumSin)
+ : alignedAtTF.get(alignedAtTF.size() - 1).pose().getRotation();
// The fused pose is the combination of translation and rotation
final Pose2d fusedPose = new Pose2d(fusedTranslation, fusedRotation);
// Fused standard deviations
final Matrix fusedStdDevs =
- VecBuilder.fill(Math.sqrt(1.0 / sumWx), Math.sqrt(1.0 / sumWy), Math.sqrt(1.0 / sumWth));
+ VecBuilder.fill(
+ Math.sqrt(1.0 / sumWx),
+ Math.sqrt(1.0 / sumWy),
+ hasAngularWeight ? Math.sqrt(1.0 / sumWth) : Double.POSITIVE_INFINITY);
// Construct and return the TimedPose objects
return new TimedPose(fusedPose, tFusion, fusedStdDevs);
@@ -692,8 +744,9 @@ private TimedPose smoothAtTime(double tFusion) {
Pose2d alignedPose = timeAlignPose(e.pose(), e.timestampSeconds(), tFusion);
if (alignedPose == null) continue;
aligned.add(new TimedPose(alignedPose, tFusion, e.stdDevs()));
- // Debugging Logging
- Logger.recordOutput("Vision/Debug/deltaTime", tFusion - e.timestampSeconds());
+ if (Constants.isTuningMode()) {
+ Logger.recordOutput("Vision/Debug/deltaTime", tFusion - e.timestampSeconds());
+ }
}
if (aligned.isEmpty()) return fusedBuffer.peekLast();
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java
index c9bbc0d3..dbbbe6f9 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java
@@ -17,20 +17,31 @@
import edu.wpi.first.networktables.DoubleArraySubscriber;
import edu.wpi.first.networktables.DoubleSubscriber;
import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.TimestampedDoubleArray;
import edu.wpi.first.wpilibj.RobotController;
+import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashSet;
-import java.util.LinkedList;
import java.util.List;
import java.util.Set;
import java.util.function.Supplier;
/** IO implementation for real Limelight hardware. */
public class VisionIOLimelight implements VisionIO {
+ private static final int BOTPOSE_MIN_LENGTH = 11;
+ private static final int BOTPOSE_LATENCY_INDEX = 6;
+ private static final int BOTPOSE_TAG_COUNT_INDEX = 7;
+ private static final int BOTPOSE_AVG_DISTANCE_INDEX = 9;
+ private static final int BOTPOSE_FIRST_TAG_ID_INDEX = 11;
+ private static final int BOTPOSE_FIRST_TAG_AMBIGUITY_INDEX = 17;
+ private static final int BOTPOSE_TAG_STRIDE = 7;
+ private static final long ORIENTATION_FLUSH_PERIOD_US = 20_000;
+ private static long lastOrientationFlushUs = 0;
+
private final Supplier rotationSupplier;
private final DoubleArrayPublisher orientationPublisher;
- private final DoubleSubscriber latencySubscriber;
+ private final DoubleSubscriber heartbeatSubscriber;
private final DoubleSubscriber txSubscriber;
private final DoubleSubscriber tySubscriber;
private final DoubleArraySubscriber megatag1Subscriber;
@@ -46,7 +57,7 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) {
var table = NetworkTableInstance.getDefault().getTable(name);
this.rotationSupplier = rotationSupplier;
orientationPublisher = table.getDoubleArrayTopic("robot_orientation_set").publish();
- latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0);
+ heartbeatSubscriber = table.getDoubleTopic("hb").subscribe(0.0);
txSubscriber = table.getDoubleTopic("tx").subscribe(0.0);
tySubscriber = table.getDoubleTopic("ty").subscribe(0.0);
megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {});
@@ -58,7 +69,7 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) {
public void updateInputs(VisionIOInputs inputs) {
// Update connection status based on whether an update has been seen in the last 250ms
inputs.connected =
- ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250;
+ ((RobotController.getFPGATime() - heartbeatSubscriber.getLastChange()) / 1000) < 250;
// Update target observation
inputs.latestTargetObservation =
@@ -68,44 +79,37 @@ public void updateInputs(VisionIOInputs inputs) {
// Update orientation for MegaTag 2
orientationPublisher.accept(
new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0});
- NetworkTableInstance.getDefault()
- .flush(); // Increases network traffic but recommended by Limelight
+ flushOrientationIfDue();
// Read new pose observations from NetworkTables
Set unionTagIds = new HashSet<>();
- List poseObservations = new LinkedList<>();
+ List poseObservations = new ArrayList<>();
for (var rawSample : megatag1Subscriber.readQueue()) {
- if (rawSample.value.length == 0) continue;
-
- int tagCount = (int) rawSample.value[7];
-
- // Build used tag array for THIS observation only
- int[] used = new int[tagCount];
- int u = 0;
+ if (!isValidBotPoseSample(rawSample.value)) continue;
- for (int i = 11; i < rawSample.value.length && u < tagCount; i += 7) {
- int id = (int) rawSample.value[i];
- used[u++] = id;
- unionTagIds.add(id);
- }
+ int tagCount = getTagCount(rawSample.value);
+ int[] used = extractUsedTagIds(rawSample.value);
+ for (int id : used) unionTagIds.add(id);
poseObservations.add(
new PoseObservation(
// Timestamp, based on server timestamp of publish and latency
- rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3,
+ timestampSeconds(rawSample),
// 3D pose estimate
parsePose(rawSample.value),
// Ambiguity, using only the first tag because ambiguity isn't applicable for multitag
- rawSample.value.length >= 18 ? rawSample.value[17] : 0.0,
+ rawSample.value.length > BOTPOSE_FIRST_TAG_AMBIGUITY_INDEX
+ ? rawSample.value[BOTPOSE_FIRST_TAG_AMBIGUITY_INDEX]
+ : 0.0,
// Tag count
tagCount,
// Average tag distance
- rawSample.value[9],
+ rawSample.value[BOTPOSE_AVG_DISTANCE_INDEX],
// Observation type
PoseObservationType.MEGATAG_1,
@@ -115,23 +119,16 @@ public void updateInputs(VisionIOInputs inputs) {
}
for (var rawSample : megatag2Subscriber.readQueue()) {
- if (rawSample.value.length == 0) continue;
+ if (!isValidBotPoseSample(rawSample.value)) continue;
- int tagCount = (int) rawSample.value[7];
-
- int[] used = new int[tagCount];
- int u = 0;
-
- for (int i = 11; i < rawSample.value.length && u < tagCount; i += 7) {
- int id = (int) rawSample.value[i];
- used[u++] = id;
- unionTagIds.add(id);
- }
+ int tagCount = getTagCount(rawSample.value);
+ int[] used = extractUsedTagIds(rawSample.value);
+ for (int id : used) unionTagIds.add(id);
poseObservations.add(
new PoseObservation(
// Timestamp, based on server timestamp of publish and latency
- rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3,
+ timestampSeconds(rawSample),
// 3D pose estimate
parsePose(rawSample.value),
@@ -140,10 +137,10 @@ public void updateInputs(VisionIOInputs inputs) {
0.0,
// Tag count
- (int) rawSample.value[7],
+ tagCount,
// Average tag distance
- rawSample.value[9],
+ rawSample.value[BOTPOSE_AVG_DISTANCE_INDEX],
// Observation type
PoseObservationType.MEGATAG_2,
@@ -164,7 +161,11 @@ public void updateInputs(VisionIOInputs inputs) {
}
/** Parses the 3D pose from a Limelight botpose array. */
- private static Pose3d parsePose(double[] rawLLArray) {
+ static Pose3d parsePose(double[] rawLLArray) {
+ if (rawLLArray.length < BOTPOSE_MIN_LENGTH) {
+ throw new IllegalArgumentException(
+ "Limelight botpose array must have at least " + BOTPOSE_MIN_LENGTH + " values.");
+ }
return new Pose3d(
rawLLArray[0],
rawLLArray[1],
@@ -174,4 +175,45 @@ private static Pose3d parsePose(double[] rawLLArray) {
Units.degreesToRadians(rawLLArray[4]),
Units.degreesToRadians(rawLLArray[5])));
}
+
+ static boolean isValidBotPoseSample(double[] rawLLArray) {
+ return rawLLArray.length >= BOTPOSE_MIN_LENGTH
+ && getTagCount(rawLLArray) >= 0
+ && Double.isFinite(rawLLArray[BOTPOSE_LATENCY_INDEX])
+ && Double.isFinite(rawLLArray[BOTPOSE_AVG_DISTANCE_INDEX]);
+ }
+
+ static int[] extractUsedTagIds(double[] rawLLArray) {
+ int tagCount = getTagCount(rawLLArray);
+ int[] used = new int[Math.max(0, tagCount)];
+ int count = 0;
+
+ for (int i = BOTPOSE_FIRST_TAG_ID_INDEX;
+ i < rawLLArray.length && count < used.length;
+ i += BOTPOSE_TAG_STRIDE) {
+ used[count++] = (int) rawLLArray[i];
+ }
+
+ return count == used.length ? used : Arrays.copyOf(used, count);
+ }
+
+ static double timestampSeconds(TimestampedDoubleArray rawSample) {
+ return timestampSeconds(rawSample.timestamp, rawSample.value);
+ }
+
+ static double timestampSeconds(long ntTimestampMicros, double[] rawLLArray) {
+ return ntTimestampMicros * 1.0e-6 - rawLLArray[BOTPOSE_LATENCY_INDEX] * 1.0e-3;
+ }
+
+ private static int getTagCount(double[] rawLLArray) {
+ return (int) rawLLArray[BOTPOSE_TAG_COUNT_INDEX];
+ }
+
+ private static void flushOrientationIfDue() {
+ long nowUs = RobotController.getFPGATime();
+ if (nowUs - lastOrientationFlushUs < ORIENTATION_FLUSH_PERIOD_US) return;
+
+ NetworkTableInstance.getDefault().flush();
+ lastOrientationFlushUs = nowUs;
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java
index 4dac1e05..0763baf7 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java
@@ -17,6 +17,7 @@
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashSet;
+import java.util.List;
import java.util.Set;
import org.photonvision.PhotonCamera;
@@ -50,10 +51,12 @@ public void updateInputs(VisionIOInputs inputs) {
Rotation2d bestYaw = Rotation2d.kZero;
Rotation2d bestPitch = Rotation2d.kZero;
- int processed = 0;
- for (var result : camera.getAllUnreadResults()) {
- // Hard cap
- if (processed++ >= kMaxUnread) break;
+ List unreadResults =
+ camera.getAllUnreadResults();
+ int startIndex = Math.max(0, unreadResults.size() - kMaxUnread);
+
+ for (int resultIndex = startIndex; resultIndex < unreadResults.size(); resultIndex++) {
+ var result = unreadResults.get(resultIndex);
final double ts = result.getTimestampSeconds();
@@ -64,8 +67,8 @@ public void updateInputs(VisionIOInputs inputs) {
}
// Add pose observation
- if (result.multitagResult.isPresent()) { // Multitag result
- var multitag = result.multitagResult.get();
+ if (result.getMultiTagResult().isPresent()) { // Multitag result
+ var multitag = result.getMultiTagResult().get();
// Calculate robot pose
Transform3d fieldToCamera = multitag.estimatedPose.best;
@@ -74,11 +77,11 @@ public void updateInputs(VisionIOInputs inputs) {
// Calculate average tag distance
double totalTagDistance = 0.0;
- for (var target : result.targets) {
- totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm();
+ List targets = result.getTargets();
+ for (var target : targets) {
+ totalTagDistance += target.getBestCameraToTarget().getTranslation().getNorm();
}
- double avgTagDistance =
- result.targets.isEmpty() ? 0.0 : (totalTagDistance / result.targets.size());
+ double avgTagDistance = targets.isEmpty() ? 0.0 : (totalTagDistance / targets.size());
// Build used tag list (loggable + replayable)
int[] used = new int[multitag.fiducialIDsUsed.size()];
@@ -99,32 +102,32 @@ public void updateInputs(VisionIOInputs inputs) {
PoseObservationType.PHOTONVISION,
used));
- } else if (!result.targets.isEmpty()) { // Single tag result
- var target = result.targets.get(0);
+ } else if (result.hasTargets()) { // Single tag result
+ var target = result.getBestTarget();
// Calculate robot pose
- var tagPose = aprilTagLayout.getTagPose(target.fiducialId);
+ var tagPose = aprilTagLayout.getTagPose(target.getFiducialId());
if (tagPose.isEmpty()) continue;
Transform3d fieldToTarget =
new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation());
- Transform3d cameraToTarget = target.bestCameraToTarget;
+ Transform3d cameraToTarget = target.getBestCameraToTarget();
Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse());
Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse());
Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation());
- unionTagIds.add(target.fiducialId);
+ unionTagIds.add(target.getFiducialId());
poseObservations.add(
new PoseObservation(
ts,
robotPose,
- target.poseAmbiguity,
+ target.getPoseAmbiguity(),
1,
cameraToTarget.getTranslation().getNorm(),
PoseObservationType.PHOTONVISION,
- new int[] {target.fiducialId}));
+ new int[] {target.getFiducialId()}));
}
}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java
index f554cc4c..f26e7024 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java
@@ -11,6 +11,7 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.wpilibj.RobotController;
import frc.robot.FieldConstants;
import java.util.function.Supplier;
import org.photonvision.simulation.PhotonCameraSim;
@@ -20,12 +21,11 @@
/** IO implementation for physics sim using PhotonVision simulator. */
public class VisionIOPhotonVisionSim extends VisionIOPhotonVision {
private static VisionSystemSim visionSim;
+ private static long lastUpdateUs = Long.MIN_VALUE;
+ private static final long MIN_UPDATE_PERIOD_US = 1_000;
private final Supplier poseSupplier;
- @SuppressWarnings("unused")
- private final PhotonCameraSim cameraSim;
-
/**
* Creates a new VisionIOPhotonVisionSim.
*
@@ -35,6 +35,22 @@ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision {
*/
public VisionIOPhotonVisionSim(
String name, Transform3d robotToCamera, Supplier poseSupplier) {
+ this(name, robotToCamera, new SimCameraProperties(), poseSupplier);
+ }
+
+ /**
+ * Creates a new VisionIOPhotonVisionSim.
+ *
+ * @param name The name of the camera (PhotonVision camera name).
+ * @param robotToCamera Camera pose relative to robot frame.
+ * @param cameraProperties Camera simulation calibration, latency, noise, and FPS.
+ * @param poseSupplier Supplier for the robot pose (field->robot) to use in simulation.
+ */
+ public VisionIOPhotonVisionSim(
+ String name,
+ Transform3d robotToCamera,
+ SimCameraProperties cameraProperties,
+ Supplier poseSupplier) {
super(name, robotToCamera);
this.poseSupplier = poseSupplier;
@@ -44,28 +60,23 @@ public VisionIOPhotonVisionSim(
visionSim.addAprilTags(FieldConstants.aprilTagLayout);
}
- // Camera properties:
- // - If you have per-camera SimCameraProperties in Constants, pass them here instead.
- // - Otherwise keep the default and tune later.
- var cameraProperties = new SimCameraProperties();
-
- // Recommended defaults (feel free to tune)
- // cameraProperties.setCalibration(1280, 800, Rotation2d.fromDegrees(100));
- // cameraProperties.setFPS(20);
- // cameraProperties.setAvgLatencyMs(35);
- // cameraProperties.setLatencyStdDevMs(5);
-
- cameraSim = new PhotonCameraSim(camera, cameraProperties);
+ PhotonCameraSim cameraSim = new PhotonCameraSim(camera, cameraProperties);
visionSim.addCamera(cameraSim, robotToCamera);
}
@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.
- visionSim.update(poseSupplier.get());
+ updateVisionSimOncePerLoop();
// Then pull results like normal (and emit PoseObservation + usedTagIds sets)
super.updateInputs(inputs);
}
+
+ private void updateVisionSimOncePerLoop() {
+ long nowUs = RobotController.getFPGATime();
+ if (nowUs - lastUpdateUs < MIN_UPDATE_PERIOD_US) return;
+
+ visionSim.update(poseSupplier.get());
+ lastUpdateUs = nowUs;
+ }
}
diff --git a/src/main/java/frc/robot/util/Alert.java b/src/main/java/frc/robot/util/Alert.java
index 202012d8..3f64e100 100644
--- a/src/main/java/frc/robot/util/Alert.java
+++ b/src/main/java/frc/robot/util/Alert.java
@@ -22,7 +22,7 @@
/** Class for managing persistent alerts to be sent over NetworkTables. */
public class Alert {
- private static Map groups = new HashMap();
+ private static final Map groups = new HashMap<>();
private final AlertType type;
private boolean active = false;
@@ -105,7 +105,7 @@ private static class SendableAlerts implements Sendable {
public String[] getStrings(AlertType type) {
Predicate activeFilter = (Alert x) -> x.type == type && x.active;
Comparator timeSorter =
- (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime);
+ (Alert a1, Alert a2) -> Double.compare(a2.activeStartTime, a1.activeStartTime);
return alerts.stream()
.filter(activeFilter)
.sorted(timeSorter)
diff --git a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java
index a2e55e61..aa9a091c 100644
--- a/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java
+++ b/src/main/java/frc/robot/util/ConcurrentTimeInterpolatableBuffer.java
@@ -22,7 +22,9 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.interpolation.Interpolator;
+import java.util.Collections;
import java.util.Map.Entry;
+import java.util.NavigableMap;
import java.util.Optional;
import java.util.OptionalDouble;
import java.util.concurrent.ConcurrentNavigableMap;
@@ -150,8 +152,8 @@ public Optional getSample(double timeSeconds) {
m_interpolatingFunc.interpolate(bottomBound.getValue(), topBound.getValue(), ratio));
}
- public Entry getLatest() {
- return m_pastSnapshots.lastEntry();
+ public Optional> getLatest() {
+ return Optional.ofNullable(m_pastSnapshots.lastEntry());
}
/**
@@ -160,6 +162,25 @@ public Entry getLatest() {
*
* @return The internal sample buffer.
*/
+ public NavigableMap getSamplesInRange(
+ double startTimeSeconds,
+ boolean startInclusive,
+ double endTimeSeconds,
+ boolean endInclusive) {
+ if (endTimeSeconds < startTimeSeconds) {
+ return Collections.emptyNavigableMap();
+ }
+ return Collections.unmodifiableNavigableMap(
+ m_pastSnapshots.subMap(startTimeSeconds, startInclusive, endTimeSeconds, endInclusive));
+ }
+
+ /**
+ * Grant access to the internal sample buffer. Prefer {@link #getSamplesInRange} for read-only
+ * access.
+ *
+ * @return The internal sample buffer.
+ */
+ @Deprecated(forRemoval = false)
public ConcurrentNavigableMap getInternalBuffer() {
return m_pastSnapshots;
}
diff --git a/src/main/java/frc/robot/util/GetJoystickValue.java b/src/main/java/frc/robot/util/GetJoystickValue.java
deleted file mode 100644
index c149845d..00000000
--- a/src/main/java/frc/robot/util/GetJoystickValue.java
+++ /dev/null
@@ -1,18 +0,0 @@
-// Copyright (c) 2024-2026 Az-FIRST
-// http://github.com/AZ-First
-//
-// 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.
-
-package frc.robot.util;
-
-/**
- * Interface needed to abstraxct away which joystick is used for driving and which for steering with
- * a swerve base. Teams may specify to use the left joystick for either driving or steering in the
- * `Constants.java` file under OperatorConstants.
- */
-@FunctionalInterface
-public interface GetJoystickValue {
- double value();
-}
diff --git a/src/main/java/frc/robot/util/LocalADStarAK.java b/src/main/java/frc/robot/util/LocalADStarAK.java
index 36ec03b8..e4353ade 100644
--- a/src/main/java/frc/robot/util/LocalADStarAK.java
+++ b/src/main/java/frc/robot/util/LocalADStarAK.java
@@ -138,7 +138,7 @@ public void fromLog(LogTable table) {
double[] pointsLogged = table.get("CurrentPathPoints", new double[0]);
List pathPoints = new ArrayList<>();
- for (int i = 0; i < pointsLogged.length; i += 2) {
+ for (int i = 0; i + 1 < pointsLogged.length; i += 2) {
pathPoints.add(
new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null));
}
diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java
index d4cb549b..b9c7b1b6 100644
--- a/src/main/java/frc/robot/util/LoggedTunableNumber.java
+++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java
@@ -18,31 +18,33 @@
import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
/**
- * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
- * value not in dashboard.
+ * Replay-safe tunable number backed by AdvantageKit NetworkTables inputs.
+ *
+ * When tuning mode is enabled, the live value is read from {@code /Tuning/...}. Otherwise, the
+ * default value is returned.
*/
public class LoggedTunableNumber implements DoubleSupplier {
- private static final String tableKey = "TunableNumbers";
+ private static final String tableKey = "/Tuning";
private final String key;
private boolean hasDefault = false;
private double defaultValue;
private LoggedNetworkNumber dashboardNumber;
- private Map lastHasChangedValues = new HashMap<>();
+ private final Map lastHasChangedValues = new HashMap<>();
/**
- * Create a new LoggedTunableNumber
+ * Create a new LoggedTunableNumber.
*
- * @param dashboardKey Key on dashboard
+ * @param dashboardKey Key under /Tuning
*/
public LoggedTunableNumber(String dashboardKey) {
this.key = tableKey + "/" + dashboardKey;
}
/**
- * Create a new LoggedTunableNumber with the default value
+ * Create a new LoggedTunableNumber with a default value.
*
- * @param dashboardKey Key on dashboard
+ * @param dashboardKey Key under /Tuning
* @param defaultValue Default value
*/
public LoggedTunableNumber(String dashboardKey, double defaultValue) {
@@ -51,45 +53,50 @@ public LoggedTunableNumber(String dashboardKey, double defaultValue) {
}
/**
- * Set the default value of the number. The default value can only be set once.
+ * Set the default value. Can only be set once.
*
* @param defaultValue The default value
*/
public void initDefault(double defaultValue) {
- if (!hasDefault) {
- hasDefault = true;
- this.defaultValue = defaultValue;
- if (Constants.tuningMode) {
- dashboardNumber = new LoggedNetworkNumber("SmartDashboard/" + key, defaultValue);
- }
+ if (hasDefault) {
+ return;
+ }
+
+ hasDefault = true;
+ this.defaultValue = defaultValue;
+
+ if (isTuningMode()) {
+ dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
}
}
/**
- * Get the current value, from dashboard if available and in tuning mode.
+ * Get the current value.
*
- * @return The current value
+ * @return Live tuning value in tuning mode, otherwise the default
*/
public double get() {
if (!hasDefault) {
return 0.0;
- } else {
- return Constants.tuningMode ? dashboardNumber.get() : defaultValue;
}
+
+ if (dashboardNumber != null) {
+ return dashboardNumber.get();
+ }
+
+ return defaultValue;
}
/**
- * Checks whether the number has changed since our last check
+ * Returns true when this value has changed since the last check by the given caller.
*
- * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
- * objects. Recommended approach is to pass the result of "hashCode()"
- * @return True if the number has changed since the last time this method was called, false
- * otherwise.
+ * @param id Unique identifier for the caller, often hashCode()
*/
public boolean hasChanged(int id) {
double currentValue = get();
Double lastValue = lastHasChangedValues.get(id);
- if (lastValue == null || currentValue != lastValue) {
+
+ if (lastValue == null || Double.compare(currentValue, lastValue) != 0) {
lastHasChangedValues.put(id, currentValue);
return true;
}
@@ -98,22 +105,20 @@ public boolean hasChanged(int id) {
}
/**
- * Runs action if any of the tunableNumbers have changed
+ * Runs an action if any of the tunable numbers changed.
*
- * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
- * objects. Recommended approach is to pass the result of "hashCode()"
- * @param action Callback to run when any of the tunable numbers have changed. Access tunable
- * numbers in order inputted in method
- * @param tunableNumbers All tunable numbers to check
+ * @param id Unique identifier for the caller, often hashCode()
+ * @param action Callback with values in the same order as provided
+ * @param tunableNumbers Tunable numbers to watch
*/
public static void ifChanged(
int id, Consumer action, LoggedTunableNumber... tunableNumbers) {
- if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+ if (Arrays.stream(tunableNumbers).anyMatch(t -> t.hasChanged(id))) {
action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
}
}
- /** Runs action if any of the tunableNumbers have changed */
+ /** Runs an action if any of the tunable numbers changed. */
public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
ifChanged(id, values -> action.run(), tunableNumbers);
}
@@ -122,4 +127,8 @@ public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tun
public double getAsDouble() {
return get();
}
+
+ private static boolean isTuningMode() {
+ return Constants.isTuningMode();
+ }
}
diff --git a/src/main/java/frc/robot/util/OverrideSwitches.java b/src/main/java/frc/robot/util/OverrideSwitches.java
index 755a66f9..cc12fa91 100644
--- a/src/main/java/frc/robot/util/OverrideSwitches.java
+++ b/src/main/java/frc/robot/util/OverrideSwitches.java
@@ -40,7 +40,7 @@ public boolean isConnected() {
/** Gets the state of a driver-side switch (0-2 from left to right). */
public boolean getDriverSwitch(int index) {
if (index < 0 || index > 2) {
- throw new RuntimeException(
+ throw new IllegalArgumentException(
"Invalid driver override index " + Integer.toString(index) + ". Must be 0-2.");
}
return consoleSwitches.getRawButton(index + 1);
@@ -49,7 +49,7 @@ public boolean getDriverSwitch(int index) {
/** Gets the state of an operator-side switch (0-4 from left to right). */
public boolean getOperatorSwitch(int index) {
if (index < 0 || index > 4) {
- throw new RuntimeException(
+ throw new IllegalArgumentException(
"Invalid operator override index " + Integer.toString(index) + ". Must be 0-4.");
}
return consoleSwitches.getRawButton(index + 8);
diff --git a/src/main/java/frc/robot/util/RBSICANHealth.java b/src/main/java/frc/robot/util/RBSICANHealth.java
index 8959020f..0da22e9c 100644
--- a/src/main/java/frc/robot/util/RBSICANHealth.java
+++ b/src/main/java/frc/robot/util/RBSICANHealth.java
@@ -29,6 +29,11 @@ public RBSICANHealth(String busName) {
bus = RBSICANBusRegistry.getLike(busName);
}
+ @Override
+ protected int getPeriodPriority() {
+ return 20;
+ }
+
/** Periodic function */
@Override
protected void rbsiPeriodic() {
diff --git a/src/main/java/frc/robot/util/RBSIIO.java b/src/main/java/frc/robot/util/RBSIIO.java
index ae72aba0..7bcdd640 100644
--- a/src/main/java/frc/robot/util/RBSIIO.java
+++ b/src/main/java/frc/robot/util/RBSIIO.java
@@ -19,26 +19,28 @@
*/
public interface RBSIIO {
- /** Define the power ports for this subsystem */
- public final int[] powerPorts = {};
+ /** Implementations may override to provide PDH ports. */
+ default int[] powerPorts() {
+ return new int[] {};
+ }
+
+ /** Return the list of PDH power ports used for this mechanism. */
+ default int[] getPowerPorts() {
+ return powerPorts();
+ }
/** Stop all the motors */
- public default void stop() {}
+ default void stop() {}
/** Set the neutral mode of the motors to COAST */
- public default void setCoast() {}
+ default void setCoast() {}
/** Set the neutral mode of the motors to BRAKE */
- public default void setBrake() {}
+ default void setBrake() {}
/** Run open loop at the specified voltage */
- public default void setVoltage(double volts) {}
+ default void setVoltage(double volts) {}
/** Run open loop at the specified duty cycle */
- public default void setPercent(double percent) {}
-
- /** Return the list of PDH power ports used for this mechanism */
- public default int[] getPowerPorts() {
- return powerPorts;
- }
+ default void setPercent(double percent) {}
}
diff --git a/src/main/java/frc/robot/util/RBSIParsing.java b/src/main/java/frc/robot/util/RBSIParsing.java
index 0215ff2c..4b41b8dc 100644
--- a/src/main/java/frc/robot/util/RBSIParsing.java
+++ b/src/main/java/frc/robot/util/RBSIParsing.java
@@ -27,9 +27,9 @@ public class RBSIParsing {
*/
public static Byte parseModuleType() {
// NOTE: This assumes all 4 modules have the same arrangement!
- Byte b_drive; // [x,x,-,-,-,-,-,-]
- Byte b_steer; // [-,-,x,x,-,-,-,-]
- Byte b_encoder; // [-,-,-,-,x,x,-,-]
+ byte b_drive; // [x,x,-,-,-,-,-,-]
+ byte b_steer; // [-,-,x,x,-,-,-,-]
+ byte b_encoder; // [-,-,-,-,x,x,-,-]
switch (kFLDriveType) {
case "falcon":
case "kraken":
@@ -43,7 +43,7 @@ public static Byte parseModuleType() {
b_drive = 0b01;
break;
default:
- throw new RuntimeException("Invalid drive motor type");
+ throw new IllegalArgumentException("Invalid drive motor type: " + kFLDriveType);
}
switch (kFLSteerType) {
case "falcon":
@@ -58,7 +58,7 @@ public static Byte parseModuleType() {
b_steer = 0b01;
break;
default:
- throw new RuntimeException("Invalid steer motor type");
+ throw new IllegalArgumentException("Invalid steer motor type: " + kFLSteerType);
}
switch (kFLEncoderType) {
case "cancoder":
@@ -70,7 +70,7 @@ public static Byte parseModuleType() {
b_encoder = 0b01;
break;
default:
- throw new RuntimeException("Invalid swerve encoder type");
+ throw new IllegalArgumentException("Invalid swerve encoder type: " + kFLEncoderType);
}
return (byte) (0b00000000 | b_drive << 6 | b_steer << 4 | b_encoder << 2);
}
diff --git a/src/main/java/frc/robot/util/RBSIPowerMonitor.java b/src/main/java/frc/robot/util/RBSIPowerMonitor.java
index ba6daadf..6c94945a 100644
--- a/src/main/java/frc/robot/util/RBSIPowerMonitor.java
+++ b/src/main/java/frc/robot/util/RBSIPowerMonitor.java
@@ -29,14 +29,17 @@
public class RBSIPowerMonitor extends VirtualSubsystem {
private final RBSISubsystem[] subsystems;
- // private final LoggedPowerDistribution m_pdm =
- // LoggedPowerDistribution.getInstance(PowerConstants.kPDMCANid, PowerConstants.kPDMType);
- ConduitApi conduit = ConduitApi.getInstance();
+ private final ConduitApi conduit = ConduitApi.getInstance();
// Define local variables
private final LoggedTunableNumber batteryCapacityAh;
private double totalAmpHours = 0.0;
+ private double totalEnergyJoules = 0.0;
private long lastTimestampUs = RobotController.getFPGATime(); // In microseconds
+ private double lastVoltage = 0.0;
+ private double lastTotalCurrent = 0.0;
+ private boolean totalCurrentOverLimit = false;
+ private boolean brownoutImminent = false;
// DRIVE and STEER motor power ports
private final int[] m_drivePowerPorts = {
@@ -71,6 +74,11 @@ public RBSIPowerMonitor(LoggedTunableNumber batteryCapacityAh, RBSISubsystem...
}
}
+ @Override
+ protected int getPeriodPriority() {
+ return 30;
+ }
+
/** Periodic Method */
@Override
public void rbsiPeriodic() {
@@ -80,14 +88,21 @@ public void rbsiPeriodic() {
// --- Read voltage & total current ---
double voltage = conduit.getPDPVoltage();
double totalCurrent = conduit.getPDPTotalCurrent();
+ lastVoltage = voltage;
+ lastTotalCurrent = totalCurrent;
+ totalCurrentOverLimit = totalCurrent > PowerConstants.kTotalMaxCurrentAmps;
// --- Safety alerts ---
- totalCurrentAlert.set(totalCurrent > PowerConstants.kTotalMaxCurrent);
- lowVoltageAlert.set(voltage < PowerConstants.kVoltageWarning);
- criticalVoltageAlert.set(voltage < PowerConstants.kVoltageCritical);
-
- for (int ch = 0; ch < conduit.getPDPChannelCount(); ch++) {
- portAlerts[ch].set(conduit.getPDPChannelCurrent(ch) > PowerConstants.kMotorPortMaxCurrent);
+ totalCurrentAlert.set(totalCurrentOverLimit);
+ lowVoltageAlert.set(voltage < PowerConstants.kWarningVoltage);
+ criticalVoltageAlert.set(voltage < PowerConstants.kCriticalVoltage);
+ Logger.recordOutput("Power/Voltage", voltage);
+ Logger.recordOutput("Power/TotalCurrent", totalCurrent);
+ Logger.recordOutput("Power/TotalCurrentOverLimit", totalCurrentOverLimit);
+
+ for (int ch = 0; ch < Math.min(conduit.getPDPChannelCount(), portAlerts.length); ch++) {
+ portAlerts[ch].set(
+ conduit.getPDPChannelCurrent(ch) > PowerConstants.kMotorPortMaxCurrentAmps);
}
// --- Battery estimation ---
@@ -96,10 +111,11 @@ public void rbsiPeriodic() {
lastTimestampUs = nowUs;
totalAmpHours += totalCurrent * dtSec / 3600.0; // accumulate amp-hours
+ double capacityAh = batteryCapacityAh.getAsDouble();
double batteryPercent =
- 100.0 * (batteryCapacityAh.getAsDouble() - totalAmpHours) / batteryCapacityAh.getAsDouble();
+ capacityAh > 0.0 ? 100.0 * (capacityAh - totalAmpHours) / capacityAh : 0.0;
- Logger.recordOutput("Power/BatteryPercentEstimate", batteryPercent);
+ Logger.recordOutput("Power/BatteryPercentEstimate", Math.max(0.0, batteryPercent));
Logger.recordOutput("Power/AmpHoursUsed", totalAmpHours);
// --- Drive & Steer aggregation ---
@@ -113,28 +129,49 @@ public void rbsiPeriodic() {
// --- Energy / power calculations ---
double totalPower = voltage * totalCurrent; // Watts
+ totalEnergyJoules += totalPower * dtSec;
Logger.recordOutput("Power/TotalPower", totalPower);
- Logger.recordOutput("Power/EnergyJoules", totalPower * dtSec);
- Logger.recordOutput("Power/EnergyWh", totalPower * dtSec / 3600.0);
+ Logger.recordOutput("Power/EnergyJoules", totalEnergyJoules);
+ Logger.recordOutput("Power/EnergyWh", totalEnergyJoules / 3600.0);
// --- Brownout prediction ---
- boolean brownoutImminent = voltage < PowerConstants.kVoltageLimiting;
+ brownoutImminent = voltage < PowerConstants.kLimitingVoltage;
Logger.recordOutput("Power/BrownoutImminent", brownoutImminent);
-
- // --- Optional hooks for current shedding ---
- if (brownoutImminent) {
- // TODO: implement automatic shedding: e.g., disable non-critical subsystems
- }
}
private void logGroupCurrent(String name, int[] ports) {
double sum = 0.0;
for (int port : ports) {
- sum += conduit.getPDPChannelCurrent(port);
+ if (port >= 0 && port < conduit.getPDPChannelCount()) {
+ sum += conduit.getPDPChannelCurrent(port);
+ }
}
- Logger.recordOutput("Power/Subsystems/" + name + "Current", sum);
+ Logger.recordOutput("Power/Subsystems/" + name + "_Current", sum);
}
- // TODO: Do something about setting priorities if drawing too much current
+ /** Returns the most recently sampled battery voltage. */
+ public double getLastVoltage() {
+ return lastVoltage;
+ }
+
+ /** Returns the most recently sampled total current draw. */
+ public double getLastTotalCurrent() {
+ return lastTotalCurrent;
+ }
+ /** Returns whether the last sampled total current exceeded the configured warning limit. */
+ public boolean isTotalCurrentOverLimit() {
+ return totalCurrentOverLimit;
+ }
+
+ /**
+ * Returns whether the last sampled voltage is below the limiting threshold.
+ *
+ * Mechanism-specific commands can use this signal to shed load intentionally. The generic
+ * power monitor should not stop motors by itself because mechanism priority is game- and
+ * robot-specific.
+ */
+ public boolean isBrownoutImminent() {
+ return brownoutImminent;
+ }
}
diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java
index b90752fb..52e79140 100644
--- a/src/main/java/frc/robot/util/RBSISubsystem.java
+++ b/src/main/java/frc/robot/util/RBSISubsystem.java
@@ -22,7 +22,11 @@
* added functionality.
*/
public abstract class RBSISubsystem extends SubsystemBase {
+ private static final int TIMING_LOG_PERIOD_LOOPS = 5;
+ private static final int[] NO_POWER_PORTS = {};
+
private final String name = getClass().getSimpleName();
+ private int timingLogLoops = 0;
/**
* Guaranteed timing wrapper (cannot be bypassed by subclasses).
@@ -38,8 +42,11 @@ public abstract class RBSISubsystem extends SubsystemBase {
public final void periodic() {
long start = System.nanoTime();
rbsiPeriodic();
- // Log the timing for this subsystem
- Logger.recordOutput("LogPeriodic/Subsystem/" + name + "MS", (System.nanoTime() - start) / 1e6);
+ if (++timingLogLoops >= TIMING_LOG_PERIOD_LOOPS) {
+ timingLogLoops = 0;
+ Logger.recordOutput(
+ "LogPeriodic/Subsystem/" + name + "MS", (System.nanoTime() - start) / 1e6);
+ }
}
/** Subclasses must implement this instead of periodic(). */
@@ -51,7 +58,6 @@ public final void periodic() {
* @return Array of power distribution module ports
*/
public int[] getPowerPorts() {
- int[] retval = {};
- return retval;
+ return NO_POWER_PORTS;
}
}
diff --git a/src/main/java/frc/robot/util/RobotDeviceId.java b/src/main/java/frc/robot/util/RobotDeviceId.java
index e51ccba1..94db5815 100644
--- a/src/main/java/frc/robot/util/RobotDeviceId.java
+++ b/src/main/java/frc/robot/util/RobotDeviceId.java
@@ -16,6 +16,7 @@
package frc.robot.util;
import com.ctre.phoenix6.CANBus;
+import java.util.Objects;
/**
* Class for wrapping Robot / CAN devices with a name and functionality. Included here are both the
@@ -50,17 +51,34 @@ public String getBus() {
/** Get the CTRE CANBus object for a named device */
public CANBus getCANBus() {
+ return RBSICANBusRegistry.getBus(m_CANBus);
+ }
- return new CANBus(m_CANBus);
+ /** Returns whether this device has a configured Power Distribution channel. */
+ public boolean hasPowerPort() {
+ return m_PowerPort != null;
}
/** Get the Power Port for a named device */
public int getPowerPort() {
+ if (m_PowerPort == null) {
+ throw new IllegalStateException(
+ "Device " + m_CANDeviceNumber + " on CAN bus '" + m_CANBus + "' has no power port.");
+ }
return m_PowerPort;
}
/** Check whether two named devices are, in fact, the same */
- public boolean equals(RobotDeviceId other) {
- return other.m_CANDeviceNumber == m_CANDeviceNumber && other.m_CANBus == m_CANBus;
+ @Override
+ public boolean equals(Object other) {
+ if (this == other) return true;
+ if (!(other instanceof RobotDeviceId otherDevice)) return false;
+ return otherDevice.m_CANDeviceNumber == m_CANDeviceNumber
+ && Objects.equals(otherDevice.m_CANBus, m_CANBus);
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_CANDeviceNumber, m_CANBus);
}
}
diff --git a/src/main/java/frc/robot/util/TimeUtil.java b/src/main/java/frc/robot/util/TimeUtil.java
index 62f3cb39..c0c4d205 100644
--- a/src/main/java/frc/robot/util/TimeUtil.java
+++ b/src/main/java/frc/robot/util/TimeUtil.java
@@ -21,13 +21,6 @@ private TimeUtil() {}
/** Always seconds, regardless of real/sim/replay timebase quirks. */
public static double now() {
- double t = Logger.getTimestamp();
-
- // Empirical: in some environments, Logger timestamp is microseconds.
- // If it looks like µs, convert to seconds.
- if (t > 1.0e6) {
- t *= 1.0e-6;
- }
- return t;
+ return Logger.getTimestamp() * 1.0e-6;
}
}
diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java
index bb99b7ea..f0783884 100644
--- a/src/main/java/frc/robot/util/VirtualSubsystem.java
+++ b/src/main/java/frc/robot/util/VirtualSubsystem.java
@@ -18,13 +18,18 @@
* Base class for virtual subsystems -- not robot hardware -- that should be treated as subsystems
*/
public abstract class VirtualSubsystem {
+ private static final int TIMING_LOG_PERIOD_LOOPS = 5;
private static final List subsystems = new ArrayList<>();
private static boolean needsSort = false;
+ private static long nextConstructionOrder = 0;
private final String name = getClass().getSimpleName();
+ private final long constructionOrder;
+ private int timingLogLoops = 0;
// Load all defined virtual subsystems into a list
public VirtualSubsystem() {
+ constructionOrder = nextConstructionOrder++;
subsystems.add(this);
needsSort = true; // a new subsystem changed ordering
}
@@ -32,7 +37,7 @@ public VirtualSubsystem() {
/**
* Override to control ordering. Lower runs earlier.
*
- * Example: IMU inputs -30, Drive odometry -20, Vision -10, Coordinator 0.
+ *
Example: IMU inputs -30, Drive odometry -20, Vision -10, telemetry/health monitors +20.
*/
protected int getPeriodPriority() {
return 0;
@@ -47,8 +52,8 @@ public static void periodicAll() {
if (needsSort) {
subsystems.sort(
Comparator.comparingInt(VirtualSubsystem::getPeriodPriority)
- // deterministic tie-break to avoid “random” order when priorities match
- .thenComparingInt(System::identityHashCode));
+ // Preserve construction order when priorities match.
+ .thenComparingLong(subsystem -> subsystem.constructionOrder));
needsSort = false;
}
@@ -72,11 +77,19 @@ public static void periodicAll() {
public final void periodic() {
long start = System.nanoTime();
rbsiPeriodic();
- // Log the timing for this subsystem
- Logger.recordOutput(
- "LogPeriodic/VirtualSubsystem/" + name + "MS", (System.nanoTime() - start) / 1e6);
+ if (++timingLogLoops >= TIMING_LOG_PERIOD_LOOPS) {
+ timingLogLoops = 0;
+ Logger.recordOutput(
+ "LogPeriodic/VirtualSubsystem/" + name + "MS", (System.nanoTime() - start) / 1e6);
+ }
}
/** Subclasses must implement this instead of periodic(). */
protected abstract void rbsiPeriodic();
+
+ static void resetForTesting() {
+ subsystems.clear();
+ needsSort = false;
+ nextConstructionOrder = 0;
+ }
}
diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java
index 1542f85d..efd43942 100644
--- a/src/main/java/frc/robot/util/YagslConstants.java
+++ b/src/main/java/frc/robot/util/YagslConstants.java
@@ -33,11 +33,12 @@ public class YagslConstants {
// Define the internal YAGSL objects we will read into
private static final File yagslDir =
- new File(Filesystem.getDeployDirectory(), DeployConstants.yagslDir);
+ new File(Filesystem.getDeployDirectory(), DeployConstants.kYagslDir);
public static final SwerveDriveJson swerveDriveJson; // Needed by the Accelerometer subsystem
private static final PIDFPropertiesJson pidfPropertiesJson;
private static final PhysicalPropertiesJson physicalPropertiesJson;
private static final ModuleJson[] moduleJsons;
+ private static final ObjectMapper mapper = new ObjectMapper();
// Use a static to read in the YAGSL JSON files with error catching
static {
@@ -48,17 +49,13 @@ public class YagslConstants {
try {
tempSwerveJson =
- new ObjectMapper()
- .readValue(new File(yagslDir, "swervedrive.json"), SwerveDriveJson.class);
+ mapper.readValue(new File(yagslDir, "swervedrive.json"), SwerveDriveJson.class);
tempPdifJson =
- new ObjectMapper()
- .readValue(
- new File(yagslDir, "modules/pidfproperties.json"), PIDFPropertiesJson.class);
+ mapper.readValue(
+ new File(yagslDir, "modules/pidfproperties.json"), PIDFPropertiesJson.class);
tempPhysicalJson =
- new ObjectMapper()
- .readValue(
- new File(yagslDir, "modules/physicalproperties.json"),
- PhysicalPropertiesJson.class);
+ mapper.readValue(
+ new File(yagslDir, "modules/physicalproperties.json"), PhysicalPropertiesJson.class);
} catch (Exception e) {
throw new RuntimeException(e);
@@ -72,8 +69,10 @@ public class YagslConstants {
for (int i = 0; i < tempModuleJsons.length; i++) {
// moduleConfigs.put(swerveDriveJson.modules[i], i);
File moduleFile = new File(yagslDir, "modules/" + swerveDriveJson.modules[i]);
- assert moduleFile.exists();
- tempModuleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class);
+ if (!moduleFile.exists()) {
+ throw new IllegalStateException("Missing YAGSL module config: " + moduleFile);
+ }
+ tempModuleJsons[i] = mapper.readValue(moduleFile, ModuleJson.class);
}
} catch (Exception e) {
throw new RuntimeException(e);
@@ -111,7 +110,7 @@ public class YagslConstants {
static List moduleList = Arrays.asList(swerveDriveJson.modules);
// Front Left
- static ModuleJson flModule = moduleJsons[moduleList.indexOf("frontleft.json")];
+ static ModuleJson flModule = moduleJsons[moduleIndex("frontleft.json")];
public static final int kFrontLeftDriveMotorId = flModule.drive.id;
public static final int kFrontLeftSteerMotorId = flModule.angle.id;
public static final int kFrontLeftEncoderId = flModule.encoder.id;
@@ -129,7 +128,7 @@ public class YagslConstants {
public static final double kFrontLeftYPosInches = flModule.location.front;
// Front Right
- static ModuleJson frModule = moduleJsons[moduleList.indexOf("frontright.json")];
+ static ModuleJson frModule = moduleJsons[moduleIndex("frontright.json")];
public static final int kFrontRightDriveMotorId = frModule.drive.id;
public static final int kFrontRightSteerMotorId = frModule.angle.id;
public static final int kFrontRightEncoderId = frModule.encoder.id;
@@ -147,7 +146,7 @@ public class YagslConstants {
public static final double kFrontRightYPosInches = frModule.location.front;
// Back Left
- static ModuleJson blModule = moduleJsons[moduleList.indexOf("backleft.json")];
+ static ModuleJson blModule = moduleJsons[moduleIndex("backleft.json")];
public static final int kBackLeftDriveMotorId = blModule.drive.id;
public static final int kBackLeftSteerMotorId = blModule.angle.id;
public static final int kBackLeftEncoderId = blModule.encoder.id;
@@ -165,7 +164,7 @@ public class YagslConstants {
public static final double kBackLeftYPosInches = blModule.location.front;
// Back Right
- static ModuleJson brModule = moduleJsons[moduleList.indexOf("backright.json")];
+ static ModuleJson brModule = moduleJsons[moduleIndex("backright.json")];
public static final int kBackRightDriveMotorId = brModule.drive.id;
public static final int kBackRightSteerMotorId = brModule.angle.id;
public static final int kBackRightEncoderId = brModule.encoder.id;
@@ -193,4 +192,13 @@ public class YagslConstants {
public static final double kSteerD = pidfPropertiesJson.angle.d;
public static final double kSteerF = pidfPropertiesJson.angle.f;
public static final double kSteerIZ = pidfPropertiesJson.angle.iz;
+
+ private static int moduleIndex(String fileName) {
+ int index = moduleList.indexOf(fileName);
+ if (index < 0) {
+ throw new IllegalStateException(
+ "YAGSL swervedrive.json is missing required module config '" + fileName + "'.");
+ }
+ return index;
+ }
}
diff --git a/src/test/CurrentLimitTests.java b/src/test/CurrentLimitTests.java
index 079ec0ea..7f686d65 100644
--- a/src/test/CurrentLimitTests.java
+++ b/src/test/CurrentLimitTests.java
@@ -14,11 +14,13 @@
import static org.junit.jupiter.api.Assertions.assertTrue;
import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.hal.HAL;
+import edu.wpi.first.units.measure.Current;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
@@ -81,10 +83,10 @@ public void testStatorLimit() {
Timer.delay(0.020);
/* Get the next update for stator current */
- statorCurrent.waitForUpdate(1);
+ double initialCurrent = waitForCurrentAbove(statorCurrent, 100.0, 1.0);
- System.out.println("Stator current is " + statorCurrent);
- assertTrue(statorCurrent.getValue() > 100); // Stator current should be in excess of 100 amps
+ System.out.println("Stator current is " + initialCurrent + " A");
+ assertTrue(initialCurrent > 100); // Stator current should be in excess of 100 amps
/* Now apply the stator current limit */
currentLimitConfigs.StatorCurrentLimitEnable = true;
@@ -97,7 +99,7 @@ public void testStatorLimit() {
statorCurrent.waitForUpdate(1);
System.out.println("Stator current is " + statorCurrent);
- assertTrue(statorCurrent.getValue() < 25); // Give some wiggle room
+ assertTrue(statorCurrent.getValueAsDouble() < 25); // Give some wiggle room
}
@Test
@@ -107,10 +109,10 @@ public void testSupplyLimit() {
/* Configure a supply limit of 20 amps */
TalonFXConfiguration toConfigure = new TalonFXConfiguration();
CurrentLimitsConfigs currentLimitConfigs = toConfigure.CurrentLimits;
- currentLimitConfigs.SupplyCurrentLimit = 5;
- currentLimitConfigs.SupplyCurrentThreshold = 10;
- currentLimitConfigs.SupplyTimeThreshold = 1.0;
- currentLimitConfigs.StatorCurrentLimitEnable = false; // Start with supply limits off
+ currentLimitConfigs.SupplyCurrentLimit = 10;
+ currentLimitConfigs.SupplyCurrentLowerLimit = 5;
+ currentLimitConfigs.SupplyCurrentLowerTime = 1.0;
+ currentLimitConfigs.SupplyCurrentLimitEnable = false; // Start with supply limits off
retryConfigApply(() -> talon.getConfigurator().apply(toConfigure));
@@ -123,7 +125,8 @@ public void testSupplyLimit() {
supplyCurrent.waitForUpdate(1);
System.out.println("Supply current is " + supplyCurrent);
- assertTrue(supplyCurrent.getValue() > 80); // Supply current should be in excess of 80 amps
+ assertTrue(
+ supplyCurrent.getValueAsDouble() > 25); // Supply current should be high before limiting
/* Now apply the supply current limit */
currentLimitConfigs.SupplyCurrentLimitEnable = true;
@@ -137,8 +140,9 @@ public void testSupplyLimit() {
System.out.println("Supply current is " + supplyCurrent);
assertTrue(
- supplyCurrent.getValue()
- > 80); // Make sure it's still over 80 amps (time hasn't exceeded 1 second in total)
+ supplyCurrent.getValueAsDouble() <= 15
+ && supplyCurrent.getValueAsDouble()
+ > 5); // Time has not exceeded the lower-limit threshold yet
/* Wait a full extra couple seconds so the limit kicks in and starts limiting us */
Timer.delay(2);
@@ -147,7 +151,7 @@ public void testSupplyLimit() {
supplyCurrent.waitForUpdate(1);
System.out.println("Supply current is " + supplyCurrent);
- assertTrue(supplyCurrent.getValue() < 10); // Give some wiggle room
+ assertTrue(supplyCurrent.getValueAsDouble() < 10); // Give some wiggle room
}
private void retryConfigApply(Supplier toApply) {
@@ -156,6 +160,17 @@ private void retryConfigApply(Supplier toApply) {
do {
finalCode = toApply.get();
} while (!finalCode.isOK() && --triesLeftOver > 0);
- assert (finalCode.isOK());
+ assertTrue(finalCode.isOK(), "Config apply failed: " + finalCode);
+ }
+
+ private double waitForCurrentAbove(
+ StatusSignal currentSignal, double threshold, double timeoutSec) {
+ double current = currentSignal.getValueAsDouble();
+ double startTime = Timer.getFPGATimestamp();
+ while (current <= threshold && Timer.getFPGATimestamp() - startTime < timeoutSec) {
+ currentSignal.waitForUpdate(0.100);
+ current = currentSignal.getValueAsDouble();
+ }
+ return current;
}
}
diff --git a/src/test/FusedCANcoderTests.java b/src/test/FusedCANcoderTests.java
index c77f4c03..2bc7eb03 100644
--- a/src/test/FusedCANcoderTests.java
+++ b/src/test/FusedCANcoderTests.java
@@ -12,6 +12,7 @@
// the WPILib BSD license file in the root directory of this project.
import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusCode;
@@ -74,8 +75,8 @@ public void testIndividualPos() {
System.out.println("Talon Pos vs expected: " + talonPos + " vs " + TALON_POSITION);
System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION);
- assertEquals(talonPos.getValue(), TALON_POSITION, SET_DELTA);
- assertEquals(cancoderPos.getValue(), CANCODER_POSITION, SET_DELTA);
+ assertEquals(talonPos.getValueAsDouble(), TALON_POSITION, SET_DELTA);
+ assertEquals(cancoderPos.getValueAsDouble(), CANCODER_POSITION, SET_DELTA);
}
@Test
@@ -119,8 +120,8 @@ public void testFusedCANcoderPos() {
/* Make sure Talon matches CANcoder, since it should be using CANcoder's position */
System.out.println("Talon Pos vs expected: " + talonPos + " vs " + CANCODER_POSITION);
System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION);
- assertEquals(talonPos.getValue(), CANCODER_POSITION, SET_DELTA);
- assertEquals(cancoderPos.getValue(), CANCODER_POSITION, SET_DELTA);
+ assertEquals(talonPos.getValueAsDouble(), CANCODER_POSITION, SET_DELTA);
+ assertEquals(cancoderPos.getValueAsDouble(), CANCODER_POSITION, SET_DELTA);
}
@Test
@@ -164,8 +165,8 @@ public void testRemoteCANcoderPos() {
/* Make sure Talon matches CANcoder, since it should be using CANcoder's position */
System.out.println("Talon Pos vs expected: " + talonPos + " vs " + CANCODER_POSITION);
System.out.println("CANcoder Pos vs expected: " + cancoderPos + " vs " + CANCODER_POSITION);
- assertEquals(talonPos.getValue(), CANCODER_POSITION, SET_DELTA);
- assertEquals(cancoderPos.getValue(), CANCODER_POSITION, SET_DELTA);
+ assertEquals(talonPos.getValueAsDouble(), CANCODER_POSITION, SET_DELTA);
+ assertEquals(cancoderPos.getValueAsDouble(), CANCODER_POSITION, SET_DELTA);
}
private void retryConfigApply(Supplier toApply) {
@@ -174,6 +175,6 @@ private void retryConfigApply(Supplier toApply) {
do {
finalCode = toApply.get();
} while (!finalCode.isOK() && --triesLeftOver > 0);
- assert (finalCode.isOK());
+ assertTrue(finalCode.isOK(), "Config apply failed: " + finalCode);
}
}
diff --git a/src/test/LatencyCompensationTests.java b/src/test/LatencyCompensationTests.java
index 24f748bb..177ae60a 100644
--- a/src/test/LatencyCompensationTests.java
+++ b/src/test/LatencyCompensationTests.java
@@ -19,6 +19,7 @@
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Timer;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
@@ -67,21 +68,20 @@ public void testLatencyCompensator() {
assertTrue(status.isOK());
/* Wait a bit longer for the latency to actually do some work */
- try {
- Thread.sleep(10);
- } catch (Exception ex) {
- }
+ Timer.delay(0.010);
/* Calculate how much latency we'd expect */
double talonLatency = talonPos.getTimestamp().getLatency();
- double compensatedTalonPos = position + (velocity * talonLatency);
+ double compensatedTalonPos =
+ talonPos.getValueAsDouble() + (talonVel.getValueAsDouble() * talonLatency);
double cancoderLatency = cancoderPos.getTimestamp().getLatency();
- double compensatedCANcoderPos = position + (velocity * cancoderLatency);
+ double compensatedCANcoderPos =
+ cancoderPos.getValueAsDouble() + (cancoderVel.getValueAsDouble() * cancoderLatency);
/* Calculate compensated values before the assert to avoid timing issue related to it */
double functionCompensatedTalon =
- BaseStatusSignal.getLatencyCompensatedValue(talonPos, talonVel);
+ BaseStatusSignal.getLatencyCompensatedValueAsDouble(talonPos, talonVel);
double functionCompensatedCANcoder =
- BaseStatusSignal.getLatencyCompensatedValue(cancoderPos, cancoderVel);
+ BaseStatusSignal.getLatencyCompensatedValueAsDouble(cancoderPos, cancoderVel);
/* Assert the two methods match */
System.out.println("Talon Pos: " + compensatedTalonPos + " - " + functionCompensatedTalon);
diff --git a/src/test/frc/robot/RobotConstantsTest.java b/src/test/frc/robot/RobotConstantsTest.java
new file mode 100644
index 00000000..352eae37
--- /dev/null
+++ b/src/test/frc/robot/RobotConstantsTest.java
@@ -0,0 +1,119 @@
+package frc.robot;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import frc.robot.subsystems.drive.SwerveConstants;
+import org.junit.jupiter.api.Test;
+
+class RobotConstantsTest {
+ @Test
+ void robotDeviceCanBusesMatchSwerveConstants() {
+ assertEquals(SwerveConstants.kFLDriveCanbus, Constants.RobotDevices.FL_DRIVE.getBus());
+ assertEquals(SwerveConstants.kFLSteerCanbus, Constants.RobotDevices.FL_ROTATION.getBus());
+ assertEquals(SwerveConstants.kFLEncoderCanbus, Constants.RobotDevices.FL_CANCODER.getBus());
+
+ assertEquals(SwerveConstants.kFRDriveCanbus, Constants.RobotDevices.FR_DRIVE.getBus());
+ assertEquals(SwerveConstants.kFRSteerCanbus, Constants.RobotDevices.FR_ROTATION.getBus());
+ assertEquals(SwerveConstants.kFREncoderCanbus, Constants.RobotDevices.FR_CANCODER.getBus());
+
+ assertEquals(SwerveConstants.kBLDriveCanbus, Constants.RobotDevices.BL_DRIVE.getBus());
+ assertEquals(SwerveConstants.kBLSteerCanbus, Constants.RobotDevices.BL_ROTATION.getBus());
+ assertEquals(SwerveConstants.kBLEncoderCanbus, Constants.RobotDevices.BL_CANCODER.getBus());
+
+ assertEquals(SwerveConstants.kBRDriveCanbus, Constants.RobotDevices.BR_DRIVE.getBus());
+ assertEquals(SwerveConstants.kBRSteerCanbus, Constants.RobotDevices.BR_ROTATION.getBus());
+ assertEquals(SwerveConstants.kBREncoderCanbus, Constants.RobotDevices.BR_CANCODER.getBus());
+ }
+
+ @Test
+ void fieldLayoutConstantsAreInternallyConsistent() {
+ assertEquals(FieldConstants.defaultAprilTagType.getLayout(), FieldConstants.aprilTagLayout);
+ assertEquals(
+ FieldConstants.defaultAprilTagType.getLayout().getFieldLength(),
+ FieldConstants.fieldLength,
+ 1e-9);
+ assertEquals(
+ FieldConstants.defaultAprilTagType.getLayout().getFieldWidth(),
+ FieldConstants.fieldWidth,
+ 1e-9);
+ assertEquals(
+ FieldConstants.defaultAprilTagType.getLayout().getTags().size(),
+ FieldConstants.aprilTagCount);
+ assertNotNull(FieldConstants.defaultAprilTagType.getLayoutString());
+ }
+
+ @Test
+ void selectedTunableConstantsAreInUsableRanges() {
+ assertPositive(Constants.OperatorConstants.kRobotRelativeNudgeSpeedMetersPerSec);
+ assertPositive(Constants.SensorConstants.kRioAccelerometerSampleRateHz);
+
+ assertPositive(Constants.DrivebaseConstants.kSysIdPreRunStopSecs);
+ assertPositive(Constants.DrivebaseConstants.kFeedforwardCharacterizationStartDelaySecs);
+ assertPositive(Constants.DrivebaseConstants.kFeedforwardCharacterizationRampRateVoltsPerSec);
+ assertPositive(Constants.DrivebaseConstants.kWheelRadiusCharacterizationStartDelaySecs);
+ assertPositive(Constants.DrivebaseConstants.kWheelRadiusCharacterizationMaxVelocityRadPerSec);
+ assertPositive(Constants.DrivebaseConstants.kWheelRadiusCharacterizationRampRateRadPerSecSq);
+ assertPositive(Constants.DrivebaseConstants.kDisabledCoastMinSeconds);
+ assertPositive(Constants.DrivebaseConstants.kDisabledVisionCoastBlendAlpha);
+
+ assertPositive(Constants.FlywheelConstants.kMaxVoltage);
+ assertPositive(Constants.FlywheelConstants.kMotionMagicAccelerationRotPerSecSq);
+ assertPositive(Constants.FlywheelConstants.kMotionMagicJerkRotPerSecCubed);
+ assertPositive(Constants.FlywheelConstants.kSimGearing);
+ assertPositive(Constants.FlywheelConstants.kSimMomentOfInertiaKgMetersSq);
+
+ assertEquals(
+ Math.min(
+ Constants.DrivebaseConstants.kDisabledVisionBlendAlpha,
+ Constants.DrivebaseConstants.kDisabledVisionCoastBlendAlpha),
+ Constants.DrivebaseConstants.kDisabledVisionCoastBlendAlpha,
+ 1e-9);
+ }
+
+ @Test
+ void constantsDoNotExposeLegacyAliasNames() {
+ assertMissing(Constants.class, "loopPeriodSecs");
+ assertMissing(Constants.class, "tuningMode");
+ assertMissing(Constants.class, "G_TO_MPS2");
+
+ assertMissing(Constants.RobotConstants.class, "kRobotMass");
+ assertMissing(Constants.RobotConstants.class, "kRobotMOI");
+ assertMissing(Constants.RobotConstants.class, "kWheelCOF");
+ assertMissing(Constants.RobotConstants.class, "kMaxWheelTorque");
+
+ assertMissing(Constants.PowerConstants.class, "kPDMType");
+ assertMissing(Constants.PowerConstants.class, "kPDMCANid");
+ assertMissing(Constants.PowerConstants.class, "kTotalMaxCurrent");
+ assertMissing(Constants.PowerConstants.class, "kMotorPortMaxCurrent");
+ assertMissing(Constants.PowerConstants.class, "kVoltageWarning");
+
+ assertMissing(Constants.DrivebaseConstants.class, "kMaxLinearSpeed");
+ assertMissing(Constants.DrivebaseConstants.class, "kPStrafe");
+ assertMissing(Constants.DrivebaseConstants.class, "kPSPin");
+ assertMissing(Constants.DrivebaseConstants.class, "kWheelLockTime");
+ assertMissing(Constants.DrivebaseConstants.class, "kHistorySize");
+
+ assertMissing(Constants.FlywheelConstants.class, "kFlywheelGearRatio");
+ assertMissing(Constants.FlywheelConstants.class, "kSreal");
+ assertMissing(Constants.FlywheelConstants.class, "kPsim");
+
+ assertMissing(Constants.VisionConstants.class, "maxAmbiguity");
+ assertMissing(Constants.VisionConstants.class, "linearStdDevBaseline");
+
+ assertMissing(Constants.DeployConstants.class, "yagslDir");
+
+ assertMissing(Constants.OperatorConstants.class, "kDeadband");
+ assertMissing(Constants.OperatorConstants.class, "kTurnConstant");
+ assertMissing(Constants.OperatorConstants.class, "kJoystickSlewLimit");
+ }
+
+ private static void assertMissing(Class constantsClass, String fieldName) {
+ assertThrows(NoSuchFieldException.class, () -> constantsClass.getDeclaredField(fieldName));
+ }
+
+ private static void assertPositive(double value) {
+ org.junit.jupiter.api.Assertions.assertTrue(value > 0.0);
+ }
+}
diff --git a/src/test/RobotContainerTest.java b/src/test/frc/robot/RobotContainerTest.java
similarity index 80%
rename from src/test/RobotContainerTest.java
rename to src/test/frc/robot/RobotContainerTest.java
index 23d042a1..c400666c 100644
--- a/src/test/RobotContainerTest.java
+++ b/src/test/frc/robot/RobotContainerTest.java
@@ -7,7 +7,7 @@
// license that can be found in the LICENSE file at
// the root directory of this project.
-package org.littletonrobotics.frc2024;
+package frc.robot;
import static org.junit.jupiter.api.Assertions.fail;
@@ -21,8 +21,7 @@ public void createRobotContainer() {
try {
new RobotContainer();
} catch (Exception e) {
- e.printStackTrace();
- fail("Failed to instantiate RobotContainer, see stack trace above.");
+ fail("Failed to instantiate RobotContainer.", e);
}
}
}
diff --git a/src/test/frc/robot/commands/DriveCommandsTests.java b/src/test/frc/robot/commands/DriveCommandsTests.java
new file mode 100644
index 00000000..1a82ac45
--- /dev/null
+++ b/src/test/frc/robot/commands/DriveCommandsTests.java
@@ -0,0 +1,63 @@
+package frc.robot.commands;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.OperatorConstants;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.imu.Imu;
+import frc.robot.subsystems.imu.ImuIOSim;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class DriveCommandsTests {
+ private static final double EPSILON = 1e-9;
+
+ @BeforeEach
+ void setupHal() {
+ HAL.initialize(500, 0);
+ DriverStationSim.setEnabled(false);
+ DriverStationSim.notifyNewData();
+ }
+
+ @Test
+ void linearVelocityAppliesDeadbandAndSquaresMagnitudeWithoutChangingDirection() {
+ assertEquals(Translation2d.kZero, DriveCommands.getLinearVelocity(0.01, 0.01));
+
+ Translation2d velocity = DriveCommands.getLinearVelocity(0.6, 0.8);
+ assertEquals(1.0, velocity.getAngle().getCos() / 0.6, 1e-6);
+ assertEquals(1.0, velocity.getAngle().getSin() / 0.8, 1e-6);
+ assertEquals(1.0, velocity.getNorm(), EPSILON);
+ }
+
+ @Test
+ void omegaAppliesDeadbandAndPreservesSignWhenSquared() {
+ double scaled = MathUtil.applyDeadband(0.5, OperatorConstants.kJoystickDeadband);
+
+ assertEquals(0.0, DriveCommands.getOmega(0.01), EPSILON);
+ assertEquals(scaled * scaled, DriveCommands.getOmega(0.5), EPSILON);
+ assertEquals(-(scaled * scaled), DriveCommands.getOmega(-0.5), EPSILON);
+ }
+
+ @Test
+ void utilityCommandFactoriesReturnCommands() {
+ Drive drive = new Drive(new Imu(new ImuIOSim()));
+
+ Command stop = DriveCommands.stop(drive);
+ Command stopWithX = DriveCommands.stopWithX(drive);
+ Command brake = DriveCommands.setBrakeMode(drive, true);
+ Command zero = DriveCommands.zeroHeadingForAlliance(drive);
+ Command nudge = DriveCommands.robotRelativeNudge(drive, 0.1, 0.0, 0.0);
+
+ assertNotNull(stop);
+ assertNotNull(stopWithX);
+ assertNotNull(brake);
+ assertNotNull(zero);
+ assertNotNull(nudge);
+ }
+}
diff --git a/src/test/frc/robot/generated/GeneratedTunerConstantsTest.java b/src/test/frc/robot/generated/GeneratedTunerConstantsTest.java
new file mode 100644
index 00000000..cda13bb4
--- /dev/null
+++ b/src/test/frc/robot/generated/GeneratedTunerConstantsTest.java
@@ -0,0 +1,78 @@
+package frc.robot.generated;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class GeneratedTunerConstantsTest {
+ private static final double EPSILON = 1e-9;
+ private static final double TEMPLATE_XY_METERS = Units.inchesToMeters(10.0);
+ private static final double TEMPLATE_DRIVE_GEAR_RATIO = 6.746031746031747;
+ private static final double TEMPLATE_STEER_GEAR_RATIO = 21.428571428571427;
+ private static final double TEMPLATE_COUPLING_RATIO = 3.5714285714285716;
+ private static final double TEMPLATE_WHEEL_RADIUS_METERS = Units.inchesToMeters(2.0);
+
+ @Test
+ void templateTunerConstantsUseZeroOffsetsAndTenInchModuleLocations() {
+ assertTemplateGeometry(
+ COMPBOTTunerConstants.FrontLeft,
+ COMPBOTTunerConstants.FrontRight,
+ COMPBOTTunerConstants.BackLeft,
+ COMPBOTTunerConstants.BackRight);
+ assertTemplateGeometry(
+ DEVBOT1TunerConstants.FrontLeft,
+ DEVBOT1TunerConstants.FrontRight,
+ DEVBOT1TunerConstants.BackLeft,
+ DEVBOT1TunerConstants.BackRight);
+ assertTemplateGeometry(
+ DEVBOT2TunerConstants.FrontLeft,
+ DEVBOT2TunerConstants.FrontRight,
+ DEVBOT2TunerConstants.BackLeft,
+ DEVBOT2TunerConstants.BackRight);
+ }
+
+ @Test
+ void templateTunerConstantsUseMatchingRatios() {
+ assertTemplateRatios(COMPBOTTunerConstants.FrontLeft);
+ assertTemplateRatios(DEVBOT1TunerConstants.FrontLeft);
+ assertTemplateRatios(DEVBOT2TunerConstants.FrontLeft);
+ }
+
+ private static void assertTemplateGeometry(
+ SwerveModuleConstants
+ frontLeft,
+ SwerveModuleConstants
+ frontRight,
+ SwerveModuleConstants
+ backLeft,
+ SwerveModuleConstants
+ backRight) {
+ assertModule(frontLeft, TEMPLATE_XY_METERS, TEMPLATE_XY_METERS);
+ assertModule(frontRight, TEMPLATE_XY_METERS, -TEMPLATE_XY_METERS);
+ assertModule(backLeft, -TEMPLATE_XY_METERS, TEMPLATE_XY_METERS);
+ assertModule(backRight, -TEMPLATE_XY_METERS, -TEMPLATE_XY_METERS);
+ }
+
+ private static void assertModule(
+ SwerveModuleConstants
+ module,
+ double expectedX,
+ double expectedY) {
+ assertEquals(0.0, module.EncoderOffset, EPSILON);
+ assertEquals(expectedX, module.LocationX, EPSILON);
+ assertEquals(expectedY, module.LocationY, EPSILON);
+ }
+
+ private static void assertTemplateRatios(
+ SwerveModuleConstants
+ module) {
+ assertEquals(TEMPLATE_DRIVE_GEAR_RATIO, module.DriveMotorGearRatio, EPSILON);
+ assertEquals(TEMPLATE_STEER_GEAR_RATIO, module.SteerMotorGearRatio, EPSILON);
+ assertEquals(TEMPLATE_COUPLING_RATIO, module.CouplingGearRatio, EPSILON);
+ assertEquals(TEMPLATE_WHEEL_RADIUS_METERS, module.WheelRadius, EPSILON);
+ }
+}
diff --git a/src/test/frc/robot/subsystems/accelerometer/RioAccelIOTests.java b/src/test/frc/robot/subsystems/accelerometer/RioAccelIOTests.java
new file mode 100644
index 00000000..89d69cb0
--- /dev/null
+++ b/src/test/frc/robot/subsystems/accelerometer/RioAccelIOTests.java
@@ -0,0 +1,21 @@
+package frc.robot.subsystems.accelerometer;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+
+import org.junit.jupiter.api.Test;
+
+class RioAccelIOTests {
+ @Test
+ void noopReportsDisconnectedZeroAcceleration() {
+ RioAccelIO.Inputs inputs = new RioAccelIO.Inputs();
+
+ RioAccelIO.noop().updateInputs(inputs);
+
+ assertFalse(inputs.connected);
+ assertEquals(0L, inputs.timestampNs);
+ assertEquals(0.0, inputs.xG);
+ assertEquals(0.0, inputs.yG);
+ assertEquals(0.0, inputs.zG);
+ }
+}
diff --git a/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java b/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java
new file mode 100644
index 00000000..fc454264
--- /dev/null
+++ b/src/test/frc/robot/subsystems/drive/DriveSubsystemTests.java
@@ -0,0 +1,101 @@
+package frc.robot.subsystems.drive;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import frc.robot.subsystems.imu.Imu;
+import frc.robot.subsystems.imu.ImuIOSim;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class DriveSubsystemTests {
+ private static final double EPSILON = 1e-9;
+
+ @BeforeEach
+ void setupHal() {
+ assertTrue(HAL.initialize(500, 0));
+ DriverStationSim.setEnabled(false);
+ DriverStationSim.notifyNewData();
+ }
+
+ @Test
+ void yawRateUsesWrappedAngleDifference() {
+ double previous = Math.toRadians(179.0);
+ double current = Math.toRadians(-179.0);
+
+ assertEquals(Math.toRadians(2.0) / 0.02, Drive.yawRateRadPerSec(previous, current, 0.02), 1e-6);
+ assertEquals(0.0, Drive.yawRateRadPerSec(previous, current, 0.0), EPSILON);
+ }
+
+ @Test
+ void poseBufferAccessorsAreSafeWhenEmptyAndInterpolateWhenPopulated() {
+ Drive drive = new Drive(new Imu(new ImuIOSim()));
+
+ assertTrue(Double.isNaN(drive.getPoseBufferOldestTime()));
+ assertTrue(Double.isNaN(drive.getPoseBufferNewestTime()));
+ assertTrue(drive.getPoseAtTime(1.0).isEmpty());
+
+ drive.poseBufferAddSample(1.0, new Pose2d(1.0, 0.0, Rotation2d.kZero));
+ drive.poseBufferAddSample(2.0, new Pose2d(3.0, 0.0, Rotation2d.kZero));
+
+ assertEquals(1.0, drive.getPoseBufferOldestTime(), EPSILON);
+ assertEquals(2.0, drive.getPoseBufferNewestTime(), EPSILON);
+ assertEquals(2.0, drive.getPoseAtTime(1.5).orElseThrow().getX(), EPSILON);
+ }
+
+ @Test
+ void modulePeriodicUsesCommonOdometryPrefix() {
+ ModuleIO fakeIo =
+ new ModuleIO() {
+ @Override
+ public void updateInputs(ModuleIOInputs inputs) {
+ inputs.driveConnected = true;
+ inputs.turnConnected = true;
+ inputs.turnEncoderConnected = true;
+ inputs.odometryTimestamps = new double[] {1.0, 2.0, 3.0};
+ inputs.odometryDrivePositionsRad = new double[] {4.0, 5.0};
+ inputs.odometryTurnPositions =
+ new Rotation2d[] {Rotation2d.kZero, Rotation2d.kCCW_Pi_2, Rotation2d.kPi};
+ }
+ };
+ Module module = new Module(fakeIo, 0);
+
+ assertDoesNotThrow(module::periodic);
+ assertEquals(2, module.getOdometryPositions().length);
+ }
+
+ @Test
+ void disabledCoastDoesNotCountBaselineSampleAsStationary() {
+ Drive drive = new Drive(new Imu(new ImuIOSim()));
+ SwerveModulePosition[] stationaryPositions =
+ new SwerveModulePosition[] {
+ new SwerveModulePosition(1.0, Rotation2d.kZero),
+ new SwerveModulePosition(1.0, Rotation2d.kZero),
+ new SwerveModulePosition(1.0, Rotation2d.kZero),
+ new SwerveModulePosition(1.0, Rotation2d.kZero)
+ };
+
+ DriverStationSim.setEnabled(true);
+ DriverStationSim.notifyNewData();
+ drive.updateDisabledCoastState(true, false, 1.0, 0.0, stationaryPositions);
+
+ DriverStationSim.setEnabled(false);
+ DriverStationSim.notifyNewData();
+ drive.updateDisabledCoastState(false, true, 1.02, 0.0, stationaryPositions);
+ for (int i = 0; i < 9; i++) {
+ drive.updateDisabledCoastState(false, true, 1.30 + i * 0.02, 0.0, stationaryPositions);
+ }
+
+ assertTrue(drive.isDisabledCoast(1.48));
+
+ drive.updateDisabledCoastState(false, true, 1.50, 0.0, stationaryPositions);
+ assertFalse(drive.isDisabledCoast(1.50));
+ }
+}
diff --git a/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java b/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java
new file mode 100644
index 00000000..7d5aac3f
--- /dev/null
+++ b/src/test/frc/robot/subsystems/flywheel_example/FlywheelSysIdTests.java
@@ -0,0 +1,55 @@
+package frc.robot.subsystems.flywheel_example;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import org.junit.jupiter.api.Test;
+
+class FlywheelSysIdTests {
+
+ @Test
+ void sysIdRoutinesUseDistinctMechanismNames() {
+ Flywheel flywheel = new Flywheel(new RecordingFlywheelIO());
+
+ assertEquals(
+ "Flywheel SysId Voltage Forward Quasistatic",
+ flywheel.sysIdVoltageQuasistatic(SysIdRoutine.Direction.kForward).getName());
+ assertEquals(
+ "Flywheel SysId Voltage Reverse Dynamic",
+ flywheel.sysIdVoltageDynamic(SysIdRoutine.Direction.kReverse).getName());
+ assertEquals(
+ "Flywheel SysId Duty Cycle Forward Quasistatic",
+ flywheel.sysIdDutyCycleQuasistatic(SysIdRoutine.Direction.kForward).getName());
+ assertEquals(
+ "Flywheel SysId Duty Cycle Reverse Dynamic",
+ flywheel.sysIdDutyCycleDynamic(SysIdRoutine.Direction.kReverse).getName());
+ }
+
+ @Test
+ void velocityCommandsConvertRpmToMechanismRadiansPerSecond() {
+ RecordingFlywheelIO io = new RecordingFlywheelIO();
+ Flywheel flywheel = new Flywheel(io);
+
+ flywheel.runVelocity(6000.0);
+ assertEquals(Units.rotationsPerMinuteToRadiansPerSecond(6000.0), io.velocityRadPerSec);
+
+ flywheel.runVelocityProfiled(3000.0);
+ assertEquals(Units.rotationsPerMinuteToRadiansPerSecond(3000.0), io.profiledVelocityRadPerSec);
+ }
+
+ private static class RecordingFlywheelIO implements FlywheelIO {
+ double velocityRadPerSec;
+ double profiledVelocityRadPerSec;
+
+ @Override
+ public void setVelocity(double velocityRadPerSec) {
+ this.velocityRadPerSec = velocityRadPerSec;
+ }
+
+ @Override
+ public void setVelocityProfiled(double velocityRadPerSec) {
+ this.profiledVelocityRadPerSec = velocityRadPerSec;
+ }
+ }
+}
diff --git a/src/test/frc/robot/subsystems/imu/ImuIOSimTests.java b/src/test/frc/robot/subsystems/imu/ImuIOSimTests.java
new file mode 100644
index 00000000..69e925fb
--- /dev/null
+++ b/src/test/frc/robot/subsystems/imu/ImuIOSimTests.java
@@ -0,0 +1,45 @@
+package frc.robot.subsystems.imu;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class ImuIOSimTests {
+ private static final double EPSILON = 1e-9;
+
+ @Test
+ void odometrySamplesDrainOncePerUpdate() {
+ ImuIOSim io = new ImuIOSim();
+ ImuIO.ImuIOInputs inputs = new ImuIO.ImuIOInputs();
+
+ io.simulationSetYawRad(1.0);
+ io.updateInputs(inputs);
+ assertEquals(1, inputs.odometryYawTimestamps.length);
+ assertEquals(1, inputs.odometryYawPositionsRad.length);
+ assertEquals(1.0, inputs.odometryYawPositionsRad[0], EPSILON);
+
+ io.simulationSetYawRad(2.0);
+ io.updateInputs(inputs);
+ assertEquals(1, inputs.odometryYawTimestamps.length);
+ assertEquals(1, inputs.odometryYawPositionsRad.length);
+ assertEquals(2.0, inputs.odometryYawPositionsRad[0], EPSILON);
+ }
+
+ @Test
+ void zeroYawClearsOldOdometrySamplesBeforeNextUpdate() {
+ ImuIOSim io = new ImuIOSim();
+ ImuIO.ImuIOInputs inputs = new ImuIO.ImuIOInputs();
+
+ io.simulationSetYawRad(1.0);
+ io.updateInputs(inputs);
+
+ io.zeroYawRad(0.25);
+ io.updateInputs(inputs);
+
+ assertTrue(inputs.connected);
+ assertEquals(0.25, inputs.yawPositionRad, EPSILON);
+ assertEquals(1, inputs.odometryYawPositionsRad.length);
+ assertEquals(0.25, inputs.odometryYawPositionsRad[0], EPSILON);
+ }
+}
diff --git a/src/test/frc/robot/subsystems/vision/VisionIOTests.java b/src/test/frc/robot/subsystems/vision/VisionIOTests.java
new file mode 100644
index 00000000..9238f0b3
--- /dev/null
+++ b/src/test/frc/robot/subsystems/vision/VisionIOTests.java
@@ -0,0 +1,77 @@
+package frc.robot.subsystems.vision;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class VisionIOTests {
+ private static final double EPSILON = 1e-9;
+
+ @Test
+ void limelightPoseParserUsesMetersAndDegrees() {
+ double[] sample = {1.2, 2.3, 0.4, 10.0, -20.0, 90.0, 35.0, 2.0, 1.5, 3.0, 0.2};
+
+ Pose3d pose = VisionIOLimelight.parsePose(sample);
+
+ assertEquals(1.2, pose.getX(), EPSILON);
+ assertEquals(2.3, pose.getY(), EPSILON);
+ assertEquals(0.4, pose.getZ(), EPSILON);
+ assertEquals(
+ new Rotation3d(
+ Units.degreesToRadians(10.0),
+ Units.degreesToRadians(-20.0),
+ Units.degreesToRadians(90.0)),
+ pose.getRotation());
+ }
+
+ @Test
+ void limelightPoseParserRejectsShortArrays() {
+ assertThrows(IllegalArgumentException.class, () -> VisionIOLimelight.parsePose(new double[6]));
+ }
+
+ @Test
+ void limelightSampleValidationRejectsMalformedSamples() {
+ assertFalse(VisionIOLimelight.isValidBotPoseSample(new double[0]));
+
+ double[] negativeTagCount = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 20.0, -1.0, 0.0, 1.0, 0.0};
+ assertFalse(VisionIOLimelight.isValidBotPoseSample(negativeTagCount));
+
+ double[] valid = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 20.0, 0.0, 0.0, 1.0, 0.0};
+ assertTrue(VisionIOLimelight.isValidBotPoseSample(valid));
+ }
+
+ @Test
+ void limelightUsedTagExtractionTrimsMissingPerTagData() {
+ double[] full = {
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 20.0, 2.0, 0.0, 1.0, 0.0, 7.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 12.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+ };
+ assertArrayEquals(new int[] {7, 12}, VisionIOLimelight.extractUsedTagIds(full));
+
+ double[] truncated = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 20.0, 2.0, 0.0, 1.0, 0.0, 7.0};
+ assertArrayEquals(new int[] {7}, VisionIOLimelight.extractUsedTagIds(truncated));
+ }
+
+ @Test
+ void limelightTimestampSubtractsTotalLatencyMilliseconds() {
+ double[] sample = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 35.0, 1.0, 0.0, 1.0, 0.0};
+
+ assertEquals(9.965, VisionIOLimelight.timestampSeconds(10_000_000L, sample), EPSILON);
+ }
+
+ @Test
+ void poseObservationCoalescesNullUsedTagsToEmptyArray() {
+ VisionIO.PoseObservation obs =
+ new VisionIO.PoseObservation(
+ 1.0, new Pose3d(), 0.0, 1, 2.0, VisionIO.PoseObservationType.PHOTONVISION, null);
+
+ assertArrayEquals(new int[0], obs.usedTagIds());
+ }
+}
diff --git a/src/test/frc/robot/util/UtilTests.java b/src/test/frc/robot/util/UtilTests.java
new file mode 100644
index 00000000..94c759f2
--- /dev/null
+++ b/src/test/frc/robot/util/UtilTests.java
@@ -0,0 +1,79 @@
+package frc.robot.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.RobotController;
+import java.util.NavigableMap;
+import org.junit.jupiter.api.Test;
+
+class UtilTests {
+ private static final double EPSILON = 1e-9;
+
+ @Test
+ void timeUtilReturnsSecondsFromAdvantageKitMicrosecondTimestamp() {
+ assertTrue(HAL.initialize(500, 0));
+
+ double expectedSeconds = RobotController.getFPGATime() * 1.0e-6;
+ double actualSeconds = TimeUtil.now();
+
+ assertEquals(expectedSeconds, actualSeconds, 0.050);
+ }
+
+ @Test
+ void robotDeviceIdUsesValueEqualityAndHandlesMissingPowerPort() {
+ RobotDeviceId first = new RobotDeviceId(7, "rio", null);
+ RobotDeviceId second = new RobotDeviceId(7, new String("rio"), null);
+ RobotDeviceId differentBus = new RobotDeviceId(7, "DriveTrain", null);
+
+ assertEquals(first, second);
+ assertEquals(first.hashCode(), second.hashCode());
+ assertFalse(first.equals(differentBus));
+ assertFalse(first.hasPowerPort());
+ assertThrows(IllegalStateException.class, first::getPowerPort);
+ }
+
+ @Test
+ void robotDeviceIdLooksUpCanBusThroughRegistry() {
+ RBSICANBusRegistry.initSim("rio");
+ RobotDeviceId device = new RobotDeviceId(1, "rio", 0);
+
+ assertThrows(IllegalStateException.class, device::getCANBus);
+ }
+
+ @Test
+ void concurrentTimeInterpolatableBufferInterpolatesAndExposesReadOnlyRanges() {
+ ConcurrentTimeInterpolatableBuffer buffer =
+ ConcurrentTimeInterpolatableBuffer.createDoubleBuffer(1.0);
+
+ assertTrue(buffer.getSample(0.0).isEmpty());
+ assertTrue(buffer.getLatest().isEmpty());
+
+ buffer.addSample(1.0, 10.0);
+ buffer.addSample(2.0, 20.0);
+
+ assertEquals(15.0, buffer.getSample(1.5).orElseThrow(), EPSILON);
+ assertEquals(10.0, buffer.getSample(0.5).orElseThrow(), EPSILON);
+ assertEquals(20.0, buffer.getSample(2.5).orElseThrow(), EPSILON);
+ assertEquals(2.0, buffer.getLatest().orElseThrow().getKey(), EPSILON);
+
+ NavigableMap range = buffer.getSamplesInRange(1.0, true, 2.0, false);
+ assertEquals(1, range.size());
+ assertEquals(10.0, range.get(1.0), EPSILON);
+ assertThrows(UnsupportedOperationException.class, () -> range.put(1.5, 15.0));
+ assertTrue(buffer.getSamplesInRange(2.0, true, 1.0, true).isEmpty());
+ }
+
+ @Test
+ void overrideSwitchesRejectInvalidIndexes() {
+ OverrideSwitches switches = new OverrideSwitches(5);
+
+ assertThrows(IllegalArgumentException.class, () -> switches.getDriverSwitch(-1));
+ assertThrows(IllegalArgumentException.class, () -> switches.getDriverSwitch(3));
+ assertThrows(IllegalArgumentException.class, () -> switches.getOperatorSwitch(-1));
+ assertThrows(IllegalArgumentException.class, () -> switches.getOperatorSwitch(5));
+ }
+}
diff --git a/src/test/frc/robot/util/VirtualSubsystemTests.java b/src/test/frc/robot/util/VirtualSubsystemTests.java
new file mode 100644
index 00000000..2d63f2cc
--- /dev/null
+++ b/src/test/frc/robot/util/VirtualSubsystemTests.java
@@ -0,0 +1,75 @@
+package frc.robot.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class VirtualSubsystemTests {
+
+ @BeforeEach
+ void resetRegistry() {
+ VirtualSubsystem.resetForTesting();
+ }
+
+ @AfterEach
+ void cleanupRegistry() {
+ VirtualSubsystem.resetForTesting();
+ }
+
+ @Test
+ void periodicAllOrdersByPriorityThenConstructionOrder() {
+ List calls = new ArrayList<>();
+
+ new RecordingSubsystem("middleA", 0, calls);
+ new RecordingSubsystem("early", -30, calls);
+ new RecordingSubsystem("middleB", 0, calls);
+ new RecordingSubsystem("late", 20, calls);
+
+ VirtualSubsystem.periodicAll();
+
+ assertEquals(List.of("early", "middleA", "middleB", "late"), calls);
+ }
+
+ @Test
+ void periodicAllResortsWhenSubsystemIsAddedAfterFirstRun() {
+ List calls = new ArrayList<>();
+
+ new RecordingSubsystem("middleA", 0, calls);
+ new RecordingSubsystem("middleB", 0, calls);
+
+ VirtualSubsystem.periodicAll();
+ calls.clear();
+
+ new RecordingSubsystem("early", -10, calls);
+
+ VirtualSubsystem.periodicAll();
+
+ assertEquals(List.of("early", "middleA", "middleB"), calls);
+ }
+
+ private static final class RecordingSubsystem extends VirtualSubsystem {
+ private final String name;
+ private final int priority;
+ private final List calls;
+
+ RecordingSubsystem(String name, int priority, List calls) {
+ this.name = name;
+ this.priority = priority;
+ this.calls = calls;
+ }
+
+ @Override
+ protected int getPeriodPriority() {
+ return priority;
+ }
+
+ @Override
+ protected void rbsiPeriodic() {
+ calls.add(name);
+ }
+ }
+}
@@ -74,8 +79,6 @@ https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/index.html). Take a
moment to update all software and firmware to the latest versions before
attempting to load your new robot project.
-
-
--------
### Setting up your new project
@@ -105,27 +108,58 @@ steps you need to complete:
```
2. If you have an all-CTRE swerve base (*i.e.*, 8x TalonFX-controlled motors,
- 4x CANCoders, and 1x Pigeon2), use Phoenix Tuner X to create a swerve
- project. Follow the instructions in the Phoenix documentation for the
+ 4x CANcoders, and 1x Pigeon2), use Phoenix Tuner X to create a swerve
+ project. Follow the instructions in CTRE's
[Tuner X Swerve Project Generator](
https://v6.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html).
- This will generate the correct offsets and inversions for your drive train.
+ This generates the measured module offsets, module locations, device IDs,
+ inversions, gear ratios, and base Phoenix 6 swerve constants for your drive
+ train.
+
+3. On the final screen in Tuner X, choose the option that generates only
+ `TunerConstants.java`.
+
+4. Copy that generated file into `src/main/java/frc/robot/generated/`, then
+ rename it for the RBSI robot selected in `Constants.java`:
+
+ - `COMPBOTTunerConstants.java` for `Constants.RobotType.COMPBOT`
+ - `DEVBOT1TunerConstants.java` for `Constants.RobotType.DEVBOT1`
+ - `DEVBOT2TunerConstants.java` for `Constants.RobotType.DEVBOT2`
+
+ Also update the class name inside the file. For example, if you copied the
+ file to `COMPBOTTunerConstants.java`, the declaration must be:
+
+ ```java
+ public class COMPBOTTunerConstants {
+ ```
+
+5. In the copied `*TunerConstants.java` file, comment out the generated import
+ for `CommandSwerveDrivetrain`. RBSI does not use CTRE's generated command
+ drivetrain class.
+
+6. In the same file, comment out the generated `createDrivetrain()` function.
+ RBSI constructs the drivebase through `frc.robot.subsystems.drive.Drive`,
+ then reads the generated `DrivetrainConstants`, `FrontLeft`, `FrontRight`,
+ `BackLeft`, and `BackRight` constants through the RBSI view classes.
-3. On the final screen in Tuner X, choose "Generate only TunerConstants" and
- overwrite the file located at `src/main/java/frc/robot/generated/TunerConstants.java`.
+7. Make sure the matching `*TunerView.java` file still points at the generated
+ constants file you copied. For example, `COMPBOTTunerView` should return
+ `COMPBOTTunerConstants.kCANBus`, `COMPBOTTunerConstants.DrivetrainConstants`,
+ and the four public module constants. `TunerFactory` selects the right view
+ from `Constants.getRobot()`, so normal drive code does not import a
+ per-robot TunerConstants class directly.
-4. In `TunerConstants.java`, comment out the [last import](
- https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/1db713d75b08a4315c9273cebf5b5e6a130ed3f7/java/SwerveWithPathPlanner/src/main/java/frc/robot/generated/TunerConstants.java#L18)
- and [last method](
- https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/1db713d75b08a4315c9273cebf5b5e6a130ed3f7/java/SwerveWithPathPlanner/src/main/java/frc/robot/generated/TunerConstants.java#L171-L175).
- Before removing them, both lines will be marked as errors in VSCode.
+8. In the copied `*TunerConstants.java`, review `kSlipCurrent`. A conservative
+ starting point is `60` amps; tune it on the real robot and event carpet.
-5. In `TunerConstants.java`, change `kSlipCurrent` to `60` amps. This will
- keep your robot from tearing holes in the carpet at competition!
+9. In the copied `*TunerConstants.java`, review `kSteerInertia` and
+ `kDriveInertia`. The generic RBSI simulation expects values close to
+ `0.004` and `0.025`, respectively, unless you have better measured values.
-6. In `TunerConstants.java`, change `kSteerInertia` to `0.004` and
- `kDriveInertia` to `0.025` to allow the AdvantageKit simulation code to
- operate as expected.
+10. Open [RBSI-Constants.md](RBSI-Constants.md) and work through the sections
+ that match your robot. At minimum, verify `RobotDevices`,
+ `DrivebaseConstants`, `OperatorConstants`, `AutoConstants`, and
+ `VisionConstants` before your first serious drive test.
**NOTE:** If you have any other combination of hardware (including REV NEOs,
@@ -139,9 +173,11 @@ repository](https://github.com/AZ-First/Az-RBSI).
--------
-### Getting Started with your Robot Code
+### Getting Started with Your Robot Code
-See the Az-RBSI [Getting Started Guide](RBSI-GSG.md) for next steps.
+See the Az-RBSI [Getting Started Guide](RBSI-GSG.md) for next steps. The
+[documentation index](README.md) also links the drivetrain, vision, autonomous,
+constants, pose-buffer, and SysId guides.
--------
diff --git a/doc/RBSI-AdvantageScope.md b/doc/RBSI-AdvantageScope.md
new file mode 100644
index 00000000..8c1e472f
--- /dev/null
+++ b/doc/RBSI-AdvantageScope.md
@@ -0,0 +1,285 @@
+# RBSI AdvantageScope Layout
+
+This page describes `AdvantageScope RBSI Standard.json`, the standard
+AdvantageScope layout included with RBSI.
+
+Use this layout when reviewing logs from practice, matches, replay, and pit
+debugging. It is intentionally generic: it focuses on drivetrain, odometry,
+vision, power, CAN health, loop timing, and the example flywheel.
+
+## Loading The Layout
+
+1. Open AdvantageScope.
+2. Open a live robot connection or a `.wpilog`.
+3. Use AdvantageScope's layout import/open command.
+4. Select `AdvantageScope RBSI Standard.json` from the repository root.
+
+The companion `AdvantageScope Swerve Calibration.json` layout is narrower and
+intended for swerve setup and calibration.
+
+## Tabs
+
+### 2D Field - Odometry
+
+Shows `/RealOutputs/Odometry/Robot` on the 2026 field. Use this first when
+checking whether the robot pose moves correctly from wheel odometry and gyro
+alone.
+
+### 2D Field - Vision
+
+Overlays robot odometry, visible AprilTag poses, and accepted camera robot-pose
+observations. Use this to confirm camera transforms and tag filtering.
+
+### Vision Acceptance
+
+Plots per-camera observation counts and vision trust diagnostics:
+
+- observations seen, accepted, and rejected,
+- tag count this loop,
+- average tag distance,
+- linear and angular standard deviations,
+- accepted/fused boolean stripes.
+
+Use this tab when vision appears connected but pose is not updating.
+
+### 2D Field - Replay
+
+Shows replay-output odometry and replay-output vision observations. Use this
+when running AdvantageKit replay to compare simulated/replayed pose behavior
+against real match data.
+
+### Real vs Replay
+
+Overlays real and replay robot poses. This is useful when validating replay
+changes to odometry, pose fusion, or autonomous behavior.
+
+### Odometry Health
+
+Plots odometry support signals:
+
+- IMU latency seconds,
+- Phoenix odometry dropped samples,
+- Spark odometry dropped samples,
+- pose reset timestamp,
+- pose reset epoch.
+
+Use this tab when pose updates stutter, replay diverges, or vision appears to be
+rejected after pose resets.
+
+### Disabled Pose Fusion
+
+Plots disabled coast and disabled vision-fusion behavior:
+
+- disabled coast active,
+- stationary loop count,
+- max wheel delta in meters,
+- yaw rate in radians per second,
+- disabled vision blend alpha,
+- disabled vision reject/init-snap booleans.
+
+Use this when the robot is disabled on the field and vision is expected to pull
+pose gently toward tag observations after the robot stops coasting.
+
+### Swerve
+
+Shows measured, setpoint, and optimized swerve module states. Use this for
+module direction, optimization, and drive command debugging.
+
+### Drive Closed Loop
+
+Plots drive closed-loop support signals where available:
+
+- drive velocity in radians per second,
+- closed-loop drive velocity in radians per second,
+- drive acceleration,
+- feedforward voltage,
+- battery voltage.
+
+Some signals are produced only by specific IO implementations. For example,
+Phoenix and Spark paths do not log every identical helper key.
+
+### Power (V / A)
+
+Separates voltage and current onto different axes:
+
+- PDH/PDP voltage,
+- total current,
+- drive current,
+- steer current,
+- example flywheel current,
+- brownout-imminent stripe.
+
+Use this during pit checks to find low battery voltage, current spikes, or an
+unexpectedly expensive mechanism.
+
+### Battery Usage
+
+Plots higher-level battery estimates:
+
+- battery percent estimate,
+- amp-hours used,
+- total power,
+- energy in watt-hours and joules,
+- brownout-imminent stripe.
+
+Use this as a trend view across practice runs or long troubleshooting sessions.
+
+### CAN Health (utilization / counts)
+
+Separates CAN utilization from error counts:
+
+- roboRIO bus utilization,
+- drivetrain CAN bus utilization,
+- receive/transmit error counts,
+- bus-off counts,
+- TX full count,
+- enabled stripe.
+
+Utilization is locked to a `0..1` range because CTRE reports it as a fraction.
+Error counts are left unlocked so spikes are visible.
+
+### Loop Time - Robot (ms)
+
+Plots robot-level loop timing in milliseconds:
+
+- full robot cycle,
+- user code,
+- log periodic,
+- garbage collection,
+- RBSI code-loop timing split into virtual subsystem and command scheduler
+ portions.
+
+Use this first when chasing 20 ms hot-loop issues.
+
+### Loop Time - Mechanisms (ms)
+
+Plots `RBSISubsystem` timing in milliseconds. The template includes:
+
+- `DriveMS`,
+- `FlywheelMS`.
+
+Add mechanism subsystem timing here as you create robot-specific mechanisms.
+
+### Loop Time - Virtual (ms)
+
+Plots `VirtualSubsystem` timing in milliseconds:
+
+- `ImuMS`,
+- `DriveOdometryMS`,
+- `VisionMS`,
+- `AccelerometerMS`,
+- `RBSICANHealthMS`,
+- `RBSIPowerMonitorMS`.
+
+Use this when virtual processing, vision, or health monitoring is suspected of
+slowing the main loop.
+
+### 3D Field
+
+Shows 3D robot and vision objects. Use this to spot obvious transform mistakes
+such as cameras below the floor, tags in the wrong place, or the robot rotated
+incorrectly.
+
+### Video
+
+Reserved for AdvantageScope video sync. Use it when match video is available
+and aligned with a `.wpilog`.
+
+## Adding Mechanisms To Power
+
+Power logging is automatic only after the mechanism reports its PDH/PDP ports
+and is passed to `RBSIPowerMonitor`.
+
+1. Add mechanism device IDs and power ports in `Constants.RobotDevices`.
+
+ ```java
+ public static final RobotDeviceId ARM_LEADER = new RobotDeviceId(20, CANBuses.RIO, 5);
+ public static final RobotDeviceId ARM_FOLLOWER = new RobotDeviceId(21, CANBuses.RIO, 6);
+ ```
+
+2. In the mechanism IO implementation, return those ports.
+
+ ```java
+ @Override
+ public int[] powerPorts() {
+ return new int[] {
+ RobotDevices.ARM_LEADER.getPowerPort(),
+ RobotDevices.ARM_FOLLOWER.getPowerPort()
+ };
+ }
+ ```
+
+3. In the subsystem, forward IO power ports through `getPowerPorts()`.
+
+ ```java
+ @Override
+ public int[] getPowerPorts() {
+ return io.getPowerPorts();
+ }
+ ```
+
+4. In `RobotContainer`, pass the subsystem to `RBSIPowerMonitor`.
+
+ ```java
+ m_power = new RBSIPowerMonitor(batteryCapacity, m_flywheel, m_arm);
+ ```
+
+5. In AdvantageScope, add the logged key to `Power (V / A)`:
+
+ ```text
+ /RealOutputs/Power/Subsystems/Arm_Current
+ ```
+
+The subsystem name comes from `getClass().getSimpleName()`. If the class is
+`Arm`, the power key is `Arm_Current`. If the class is `CoralIntake`, the key is
+`CoralIntake_Current`.
+
+## Adding Mechanisms To Loop Timing
+
+Mechanism timing is automatic for classes that extend `RBSISubsystem` and put
+their periodic work in `rbsiPeriodic()`.
+
+1. Make the mechanism extend `RBSISubsystem`.
+
+ ```java
+ public class Arm extends RBSISubsystem {
+ @Override
+ protected void rbsiPeriodic() {
+ io.updateInputs(inputs);
+ Logger.processInputs("Arm", inputs);
+ }
+ }
+ ```
+
+2. Do not override `periodic()`. RBSI owns `periodic()` so it can time every
+ subsystem consistently.
+
+3. After deploying, add the mechanism timing key to `Loop Time - Mechanisms
+ (ms)`:
+
+ ```text
+ /RealOutputs/LogPeriodic/Subsystem/ArmMS
+ ```
+
+Again, the key uses the Java class simple name. `CoralIntake` becomes:
+
+```text
+/RealOutputs/LogPeriodic/Subsystem/CoralIntakeMS
+```
+
+Virtual subsystems use the same pattern but appear under:
+
+```text
+/RealOutputs/LogPeriodic/VirtualSubsystem/