From 07fe216ba625b7403ca2aa8e271f7c46fb33d5aa Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sat, 24 Jan 2026 14:16:08 -0700 Subject: [PATCH 01/14] Implement auto-logging of each subsystem's periodic() MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Implement rbsiPeriodic() in RBSISubsystem that child classes must override for periodic input updating and other tasks. The upshot of this is that subsystems would need to have a `rbsiPeriodic()` method for the input updating and other tasks, rather than a `periodic()`. The autologging of periodic timing is a major 👍! --- .../subsystems/flywheel_example/Flywheel.java | 3 ++- .../java/frc/robot/util/RBSISubsystem.java | 24 ++++++++++++++++++- 2 files changed, 25 insertions(+), 2 deletions(-) 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 be7ab97c..35242eeb 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -61,8 +61,9 @@ public Flywheel(FlywheelIO io) { new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); } + /** Periodic function -- inherits timing logic from RBSISubsystem */ @Override - public void periodic() { + public void rbsiPeriodic() { io.updateInputs(inputs); Logger.processInputs("Flywheel", inputs); } diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java index 97556753..2eee8a68 100644 --- a/src/main/java/frc/robot/util/RBSISubsystem.java +++ b/src/main/java/frc/robot/util/RBSISubsystem.java @@ -13,6 +13,7 @@ package frc.robot.util; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; /** * This class is designed to include Az-RBSI specific methods on top of the standard WPILib @@ -20,7 +21,28 @@ * etc.) should subclass ``RBSISubsystem`` rather than ``SubsystemBase`` in order to gain access to * added functionality. */ -public class RBSISubsystem extends SubsystemBase { +public abstract class RBSISubsystem extends SubsystemBase { + private final String name = getClass().getSimpleName(); + + /** + * Guaranteed timing wrapper (cannot be bypassed by subclasses). + * + *

DO NOT OVERRIDE. + * + *

Subsystems must implement {@link #rbsiPeriodic()} instead. + * + *

If you see a compiler error here, remove your periodic() override and move your logic into + * rbsiPeriodic(). + */ + @Deprecated(forRemoval = false) + public final void periodic() { + long start = System.nanoTime(); + rbsiPeriodic(); + Logger.recordOutput("LoggedRobot/" + name + "CodeMS", System.nanoTime() - start / 1e6); + } + + /** Subclasses must implement this instead of periodic(). */ + protected abstract void rbsiPeriodic(); /** * Gets the power ports associated with this Subsystem. From cfdbb2787e23ef4e37f3ecc09915d1d9f5ac1a1c Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 25 Jan 2026 01:43:59 -0700 Subject: [PATCH 02/14] Move drivetrain characterizable values to Constants Some characterizable constants like slip current and wheel radius have been moved to the ``DrivetrainConstants`` section of the constants file to make it easier to modify them during drivetrain tuning. Conformal changes to allow downstream modules to properly find these values. --- src/main/java/frc/robot/Constants.java | 11 +- src/main/java/frc/robot/RobotContainer.java | 7 +- .../frc/robot/generated/TunerConstants.java | 162 +++++++++--------- .../frc/robot/subsystems/drive/Module.java | 2 +- .../subsystems/drive/ModuleIOBlended.java | 5 +- .../subsystems/drive/ModuleIOTalonFX.java | 4 +- .../subsystems/drive/ModuleIOTalonFXS.java | 2 +- .../subsystems/drive/SwerveConstants.java | 9 - .../java/frc/robot/util/YagslConstants.java | 2 - 9 files changed, 102 insertions(+), 102 deletions(-) mode change 100755 => 100644 src/main/java/frc/robot/generated/TunerConstants.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9e33d5d6..c8f08023 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -265,6 +265,13 @@ public static final class DrivebaseConstants { // Set 3/4 of a rotation per second as the max angular velocity (radians/sec) public static final double kMaxAngularSpeed = 1.5 * Math.PI; + // Slip Current -- the current draw when the wheels start to slip + // Measure this against a wall. CHECK WITH THE CARPET AT AN ACTUAL EVENT!!! + public static final double kSlipCurrent = 17.0; // Amps + + // Characterized Wheel Radius (using the "Drive Wheel Radius Characterization" auto routine) + public static final double kWheelRadiusMeters = Inches.of(1.900).in(Meters); + // Maximum chassis accelerations desired for robot motion -- metric / radians // TODO: Compute the maximum linear acceleration given the PHYSICS of the ROBOT! public static final double kMaxLinearAccel = 4.0; // m/s/s @@ -360,11 +367,11 @@ public static final class AutoConstants { RobotConstants.kRobotMass.in(Kilograms), RobotConstants.kRobotMOI, new ModuleConfig( - SwerveConstants.kWheelRadiusMeters, + DrivebaseConstants.kWheelRadiusMeters, DrivebaseConstants.kMaxLinearSpeed, RobotConstants.kWheelCOF, DCMotor.getKrakenX60Foc(1).withReduction(SwerveConstants.kDriveGearRatio), - SwerveConstants.kDriveSlipCurrent, + DrivebaseConstants.kSlipCurrent, 1), Drive.getModuleTranslations()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce35545e..624d0784 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,7 +64,6 @@ import frc.robot.util.OverrideSwitches; import frc.robot.util.RBSIEnum.AutoType; import frc.robot.util.RBSIEnum.Mode; -import frc.robot.util.RBSIPowerMonitor; import java.util.Set; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonCamera; @@ -98,8 +97,8 @@ public class RobotContainer { @SuppressWarnings("unused") private final Accelerometer m_accel; - @SuppressWarnings("unused") - private final RBSIPowerMonitor m_power; + // @SuppressWarnings("unused") + // private final RBSIPowerMonitor m_power; @SuppressWarnings("unused") private final Vision m_vision; @@ -217,7 +216,7 @@ public RobotContainer() { // In addition to the initial battery capacity from the Dashbaord, ``RBSIPowerMonitor`` takes // all the non-drivebase subsystems for which you wish to have power monitoring; DO NOT // include ``m_drivebase``, as that is automatically monitored. - m_power = new RBSIPowerMonitor(batteryCapacity, m_flywheel); + // m_power = new RBSIPowerMonitor(batteryCapacity, m_flywheel); // Set up the SmartDashboard Auto Chooser based on auto type switch (Constants.getAutoType()) { diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java old mode 100755 new mode 100644 index 59ecd1bf..2102b494 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -4,18 +4,12 @@ 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 Tuner X Swerve Project Generator +// 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 TunerConstants { // Both sets of gains need to be tuned to your individual robot. @@ -241,76 +235,86 @@ public class TunerConstants { // ); // } - /** 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); - } - } + // /** + // * 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/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 4bd9017b..97d4194f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -9,7 +9,7 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.SwerveConstants.*; +import static frc.robot.Constants.DrivebaseConstants.*; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 10ff0196..8d46b809 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -10,6 +10,7 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static frc.robot.Constants.DrivebaseConstants.*; import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; @@ -199,8 +200,8 @@ public ModuleIOBlended(int module) { .withKV(DrivebaseConstants.kDriveV) .withKA(DrivebaseConstants.kDriveA); driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = SwerveConstants.kDriveSlipCurrent; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -SwerveConstants.kDriveSlipCurrent; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DrivebaseConstants.kSlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DrivebaseConstants.kSlipCurrent; driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index a8470d86..b9614d20 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -150,8 +150,8 @@ public ModuleIOTalonFX(int module) { .withKV(DrivebaseConstants.kDriveV) .withKA(DrivebaseConstants.kDriveA); driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = SwerveConstants.kDriveSlipCurrent; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -SwerveConstants.kDriveSlipCurrent; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DrivebaseConstants.kSlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DrivebaseConstants.kSlipCurrent; driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index 9e101cdc..5a809976 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -147,7 +147,7 @@ public ModuleIOTalonFXS( driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.Slot0 = constants.DriveMotorGains; driveConfig.ExternalFeedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = DrivebaseConstants.kSlipCurrent; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing OpenLoopRampsConfigs openRamps = new OpenLoopRampsConfigs(); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index 61201ddd..57e89d51 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -32,8 +32,6 @@ public class SwerveConstants { public static final double kCoupleRatio; public static final double kDriveGearRatio; public static final double kSteerGearRatio; - public static final double kWheelRadiusInches; - public static final double kWheelRadiusMeters; public static final String kCANbusName; public static final CANBus kCANBus; public static final int kPigeonId; @@ -43,7 +41,6 @@ public class SwerveConstants { public static final double kDriveFrictionVoltage; public static final double kSteerCurrentLimit; public static final double kDriveCurrentLimit; - public static final double kDriveSlipCurrent; public static final int kFLDriveMotorId; public static final int kFLSteerMotorId; public static final int kFLEncoderId; @@ -113,8 +110,6 @@ public class SwerveConstants { kCoupleRatio = TunerConstants.FrontLeft.CouplingGearRatio; kDriveGearRatio = TunerConstants.FrontLeft.DriveMotorGearRatio; kSteerGearRatio = TunerConstants.FrontLeft.SteerMotorGearRatio; - kWheelRadiusMeters = TunerConstants.FrontLeft.WheelRadius; - kWheelRadiusInches = Units.metersToInches(kWheelRadiusMeters); kCANbusName = TunerConstants.DrivetrainConstants.CANBusName; kCANBus = new CANBus(TunerConstants.DrivetrainConstants.CANBusName); kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id; @@ -124,7 +119,6 @@ public class SwerveConstants { kDriveFrictionVoltage = 0.1; kSteerCurrentLimit = 40.0; // Example from CTRE documentation kDriveCurrentLimit = 120.0; // Example from CTRE documentation - kDriveSlipCurrent = TunerConstants.FrontLeft.SlipCurrent; // Front Left kFLDriveMotorId = TunerConstants.FrontLeft.DriveMotorId; kFLSteerMotorId = TunerConstants.FrontLeft.SteerMotorId; @@ -198,8 +192,6 @@ public class SwerveConstants { kCoupleRatio = YagslConstants.kCoupleRatio; kDriveGearRatio = YagslConstants.kDriveGearRatio; kSteerGearRatio = YagslConstants.kSteerGearRatio; - kWheelRadiusInches = YagslConstants.kWheelRadiusInches; - kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); kCANbusName = YagslConstants.kCANbusName; kCANBus = new CANBus(YagslConstants.kCANbusName); kPigeonId = YagslConstants.kPigeonId; @@ -209,7 +201,6 @@ public class SwerveConstants { kDriveFrictionVoltage = YagslConstants.kDriveFrictionVoltage; kSteerCurrentLimit = YagslConstants.kSteerCurrentLimit; kDriveCurrentLimit = YagslConstants.kDriveCurrentLimit; - kDriveSlipCurrent = 120.0; // Front Left kFLDriveMotorId = YagslConstants.kFrontLeftDriveMotorId; kFLSteerMotorId = YagslConstants.kFrontLeftSteerMotorId; diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java index 9a79f9fa..1542f85d 100644 --- a/src/main/java/frc/robot/util/YagslConstants.java +++ b/src/main/java/frc/robot/util/YagslConstants.java @@ -91,8 +91,6 @@ public class YagslConstants { physicalPropertiesJson.conversionFactors.drive.gearRatio; public static final double kSteerGearRatio = physicalPropertiesJson.conversionFactors.angle.gearRatio; - public static final double kWheelRadiusInches = - physicalPropertiesJson.conversionFactors.drive.diameter / 2.0; public static final String kCANbusName = swerveDriveJson.imu.canbus; public static final int kPigeonId = swerveDriveJson.imu.id; From 27bc42ac8252c7519f116b39b3574dec8467c351 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 25 Jan 2026 01:55:26 -0700 Subject: [PATCH 03/14] Catching typos --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java | 3 +-- .../java/frc/robot/subsystems/flywheel_example/Flywheel.java | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c8f08023..27d08499 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -267,10 +267,10 @@ public static final class DrivebaseConstants { // Slip Current -- the current draw when the wheels start to slip // Measure this against a wall. CHECK WITH THE CARPET AT AN ACTUAL EVENT!!! - public static final double kSlipCurrent = 17.0; // Amps + public static final double kSlipCurrent = 20.0; // Amps // Characterized Wheel Radius (using the "Drive Wheel Radius Characterization" auto routine) - public static final double kWheelRadiusMeters = Inches.of(1.900).in(Meters); + public static final double kWheelRadiusMeters = Inches.of(2.000).in(Meters); // Maximum chassis accelerations desired for robot motion -- metric / radians // TODO: Compute the maximum linear acceleration given the PHYSICS of the ROBOT! diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 8d46b809..d9029480 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -10,7 +10,6 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static frc.robot.Constants.DrivebaseConstants.*; import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; @@ -448,7 +447,7 @@ public void setTurnPosition(Rotation2d rotation) { .withDriveMotorGearRatio(kDriveGearRatio) .withSteerMotorGearRatio(kSteerGearRatio) .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadiusMeters) + .withWheelRadius(DrivebaseConstants.kWheelRadiusMeters) .withSteerInertia(kSteerInertia) .withDriveInertia(kDriveInertia) .withSteerFrictionVoltage(kSteerFrictionVoltage) 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 35242eeb..8b934be3 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -63,7 +63,7 @@ public Flywheel(FlywheelIO io) { /** Periodic function -- inherits timing logic from RBSISubsystem */ @Override - public void rbsiPeriodic() { + protected void rbsiPeriodic() { io.updateInputs(inputs); Logger.processInputs("Flywheel", inputs); } From d726cafc03bcc6277bf04ac182a704c6734f8be6 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 25 Jan 2026 10:32:42 -0700 Subject: [PATCH 04/14] Uncomment parts of TunerConstants for cleaner diff --- .../frc/robot/generated/TunerConstants.java | 160 +++++++++--------- 1 file changed, 78 insertions(+), 82 deletions(-) diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 2102b494..3e23fec1 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -4,11 +4,17 @@ 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 TunerConstants { @@ -235,86 +241,76 @@ public class TunerConstants { // ); // } - // /** - // * 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 - // ); - // } - // } + /** 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); + } + } } From 877745bf7579f06e7bc36970b2d867468ef4243f Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 25 Jan 2026 11:22:30 -0700 Subject: [PATCH 05/14] Define a single rotation ProfiledPIDController Instead of different commands having their own ProfiledPIDController for robot rotation, define it in `Drive.java` and use it everywhere else. This works since the drivebase can only be doing one command at a time, so there is no need to have multiple controllers running around. Also, streamline where constants are defined and used to minimize repeated definitions and contain all drivebase-related constants in the ``DrivebaseConstants`` class. --- src/main/java/frc/robot/Constants.java | 19 ++++---- src/main/java/frc/robot/Robot.java | 12 ++++-- src/main/java/frc/robot/RobotContainer.java | 7 +-- .../frc/robot/commands/AutopilotCommands.java | 27 ++++-------- .../frc/robot/commands/DriveCommands.java | 25 +++-------- .../frc/robot/subsystems/drive/Drive.java | 43 +++++++++++++------ .../frc/robot/subsystems/drive/Module.java | 12 +++--- 7 files changed, 72 insertions(+), 73 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 27d08499..82a79138 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -277,11 +277,16 @@ public static final class DrivebaseConstants { public static final double kMaxLinearAccel = 4.0; // m/s/s public static final double kMaxAngularAccel = Degrees.of(720).in(Radians); - // For Profiled PID Motion, these are the rotational PID Constants: - // TODO: Need tuning! - public static final double kPTheta = 5.0; - public static final double kITheta = 0.0; - public static final double kDTheta = 0.0; + // For Profiled PID Motion -- NEED TUNING! + // Used in a variety of contexts, including PathPlanner and AutoPilot + // Chassis (not module) across-the-field strafing motion + public static final double kPStrafe = 5.0; + public static final double kIStrafe = 0.0; + public static final double kDStrafe = 0.0; + // Chassis (not module) solid-body rotation + public static final double kPSPin = 5.0; + public static final double kISPin = 0.0; + public static final double kDSpin = 0.0; // Hold time on motor brakes when disabled public static final double kWheelLockTime = 10; // seconds @@ -357,10 +362,6 @@ public static final class FlywheelConstants { public static final class AutoConstants { // ********** PATHPLANNER CONSTANTS ******************* - // Drive and Turn PID constants used for PathPlanner - public static final PIDConstants kPPdrivePID = new PIDConstants(5.0, 0.0, 0.0); - public static final PIDConstants kPPsteerPID = new PIDConstants(5.0, 0.0, 0.0); - // PathPlanner Config constants public static final RobotConfig kPathPlannerConfig = new RobotConfig( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 83d5b198..0ebfbe54 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -140,7 +140,8 @@ public void robotPeriodic() { @Override public void disabledInit() { // Set the brakes to stop robot motion - m_robotContainer.setMotorBrake(true); + m_robotContainer.getDrivebase().setMotorBrake(true); + m_robotContainer.getDrivebase().resetHeadingController(); m_disabledTimer.reset(); m_disabledTimer.start(); } @@ -150,7 +151,7 @@ public void disabledInit() { public void disabledPeriodic() { // After WHEEL_LOCK_TIME has elapsed, release the drive brakes if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.kWheelLockTime)) { - m_robotContainer.setMotorBrake(false); + m_robotContainer.getDrivebase().setMotorBrake(false); m_disabledTimer.stop(); } } @@ -161,7 +162,8 @@ public void autonomousInit() { // Just in case, cancel all running commands CommandScheduler.getInstance().cancelAll(); - m_robotContainer.setMotorBrake(true); + m_robotContainer.getDrivebase().setMotorBrake(true); + m_robotContainer.getDrivebase().resetHeadingController(); // TODO: Make sure Gyro inits here with whatever is in the path planning thingie switch (Constants.getAutoType()) { @@ -202,7 +204,8 @@ public void teleopInit() { } else { CommandScheduler.getInstance().cancelAll(); } - m_robotContainer.setMotorBrake(true); + m_robotContainer.getDrivebase().setMotorBrake(true); + m_robotContainer.getDrivebase().resetHeadingController(); // In case this got set in sequential practice sessions or whatever FieldState.wonAuto = null; @@ -246,6 +249,7 @@ public void teleopPeriodic() { public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + m_robotContainer.getDrivebase().resetHeadingController(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 624d0784..8bb188e4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -441,11 +441,6 @@ public void getAutonomousCommandChoreo() { RobotModeTriggers.autonomous().whileTrue(autoChooserChoreo.selectedCommandScheduler()); } - /** Set the motor neutral mode to BRAKE / COAST for T/F */ - public void setMotorBrake(boolean brake) { - m_drivebase.setMotorBrake(brake); - } - /** Updates the alerts. */ public void updateAlerts() { // AprilTag layout alert @@ -459,7 +454,7 @@ public void updateAlerts() { } } - /** Drivetrain getter method */ + /** Drivetrain getter method for use with Robot.java */ public Drive getDrivebase() { return m_drivebase; } diff --git a/src/main/java/frc/robot/commands/AutopilotCommands.java b/src/main/java/frc/robot/commands/AutopilotCommands.java index 3419d5ef..80901f83 100644 --- a/src/main/java/frc/robot/commands/AutopilotCommands.java +++ b/src/main/java/frc/robot/commands/AutopilotCommands.java @@ -21,16 +21,13 @@ import com.therekrab.autopilot.APTarget; import com.therekrab.autopilot.Autopilot.APResult; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DrivebaseConstants; import frc.robot.subsystems.drive.Drive; import org.littletonrobotics.junction.Logger; @@ -178,16 +175,6 @@ public static Command runAutopilot( */ private static Command autopilotToTarget(Drive drive, APTarget target) { - // Create PID controller - ProfiledPIDController angleController = - new ProfiledPIDController( - AutoConstants.kPPsteerPID.kP, - AutoConstants.kPPsteerPID.kI, - AutoConstants.kPPsteerPID.kD, - new TrapezoidProfile.Constraints( - DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); - angleController.enableContinuousInput(-Math.PI, Math.PI); - return Commands.run( () -> { ChassisSpeeds robotRelativeSpeeds = drive.getChassisSpeeds(); @@ -214,15 +201,19 @@ private static Command autopilotToTarget(Drive drive, APTarget target) { output.vx(), output.vy(), RadiansPerSecond.of( - angleController.calculate( - drive.getHeading().getRadians(), output.targetAngle().getRadians()))); + drive + .getAngleController() + .calculate( + drive.getHeading().getRadians(), + output.targetAngle().getRadians()))); drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, drive.getHeading())); }, drive) - // Reset PID controller when command starts - .beforeStarting(() -> angleController.reset(drive.getHeading().getRadians())) - .until(() -> AutoConstants.kAutopilot.atTarget(drive.getPose(), target)); + // Reset PID controller when command starts & ends; run until we're at target + .beforeStarting(() -> drive.resetHeadingController()) + .until(() -> AutoConstants.kAutopilot.atTarget(drive.getPose(), target)) + .finallyDo(() -> drive.resetHeadingController()); } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 9b538e10..0a2a5e73 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -10,22 +10,18 @@ package frc.robot.commands; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.SwerveConstants; @@ -117,16 +113,6 @@ public static Command fieldRelativeDriveAtAngle( DoubleSupplier ySupplier, Supplier rotationSupplier) { - // Create PID controller - ProfiledPIDController angleController = - new ProfiledPIDController( - AutoConstants.kPPsteerPID.kP, - AutoConstants.kPPsteerPID.kI, - AutoConstants.kPPsteerPID.kD, - new TrapezoidProfile.Constraints( - DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); - angleController.enableContinuousInput(-Math.PI, Math.PI); - // Construct command return Commands.run( () -> { @@ -136,8 +122,10 @@ public static Command fieldRelativeDriveAtAngle( // Calculate angular speed double omega = - angleController.calculate( - drive.getHeading().getRadians(), rotationSupplier.get().getRadians()); + drive + .getAngleController() + .calculate( + drive.getHeading().getRadians(), rotationSupplier.get().getRadians()); // Convert to field relative speeds & send command ChassisSpeeds speeds = @@ -157,8 +145,9 @@ public static Command fieldRelativeDriveAtAngle( }, drive) - // Reset PID controller when command starts - .beforeStarting(() -> angleController.reset(drive.getHeading().getRadians())); + // Reset PID controller when command starts & ends; + .beforeStarting(() -> drive.resetHeadingController()) + .finallyDo(() -> drive.resetHeadingController()); } /** Utility functions needed by commands in this module ****************** */ diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 91129b73..0a1435e8 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -15,6 +15,7 @@ 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; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; @@ -80,13 +81,7 @@ public class Drive extends SubsystemBase { private SwerveDrivePoseEstimator m_PoseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, Pose2d.kZero); - private final ProfiledPIDController thetaController = - new ProfiledPIDController( - DrivebaseConstants.kPTheta, - DrivebaseConstants.kITheta, - DrivebaseConstants.kDTheta, - new TrapezoidProfile.Constraints( - DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); + private ProfiledPIDController angleController; private DriveSimPhysics simPhysics; @@ -94,6 +89,17 @@ public class Drive extends SubsystemBase { public Drive(ImuIO imuIO) { this.imuIO = imuIO; + // Define the Angle Controller + angleController = + new ProfiledPIDController( + DrivebaseConstants.kPSPin, + DrivebaseConstants.kISPin, + DrivebaseConstants.kDSpin, + new TrapezoidProfile.Constraints( + DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + // If REAL (i.e., NOT simulation), parse out the module types if (Constants.getMode() == Mode.REAL) { // Case out the swerve types because Az-RBSI supports a lot @@ -167,7 +173,15 @@ public Drive(ImuIO imuIO) { this::resetPose, this::getChassisSpeeds, (speeds, feedforwards) -> runVelocity(speeds), - new PPHolonomicDriveController(AutoConstants.kPPdrivePID, AutoConstants.kPPsteerPID), + new PPHolonomicDriveController( + new PIDConstants( + DrivebaseConstants.kPStrafe, + DrivebaseConstants.kIStrafe, + DrivebaseConstants.kDStrafe), + new PIDConstants( + DrivebaseConstants.kPSPin, + DrivebaseConstants.kISPin, + DrivebaseConstants.kDSpin)), AutoConstants.kPathPlannerConfig, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this); @@ -377,14 +391,17 @@ public void runCharacterization(double output) { } /** - * Reset the heading ProfiledPIDController - * - *

TODO: CALL THIS FUNCTION!!! + * Reset the heading for the ProfiledPIDController * *

Call this when: (A) robot is disabled, (B) gyro is zeroed, (C) autonomous starts */ public void resetHeadingController() { - thetaController.reset(getHeading().getRadians()); + angleController.reset(getHeading().getRadians()); + } + + /** Getter function for the angle controller */ + public ProfiledPIDController getAngleController() { + return angleController; } /** SysId Characterization Routines ************************************** */ @@ -523,11 +540,13 @@ public void zeroHeadingForAlliance() { DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? Rotation2d.kZero : Rotation2d.k180deg); + resetHeadingController(); } /** Zeros the heading */ public void zeroHeading() { imuIO.zeroYaw(Rotation2d.kZero); + resetHeadingController(); } /** Adds a new timestamped vision measurement. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 97d4194f..abc40ee2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -9,14 +9,13 @@ package frc.robot.subsystems.drive; -import static frc.robot.Constants.DrivebaseConstants.*; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import frc.robot.Constants.DrivebaseConstants; import org.littletonrobotics.junction.Logger; public class Module { @@ -53,7 +52,8 @@ public void periodic() { int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together odometryPositions = new SwerveModulePosition[sampleCount]; for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] * kWheelRadiusMeters; + double positionMeters = + inputs.odometryDrivePositionsRad[i] * DrivebaseConstants.kWheelRadiusMeters; Rotation2d angle = inputs.odometryTurnPositions[i]; odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); } @@ -71,7 +71,7 @@ public void runSetpoint(SwerveModuleState state) { state.cosineScale(inputs.turnPosition); // Apply setpoints - io.setDriveVelocity(state.speedMetersPerSecond / kWheelRadiusMeters); + io.setDriveVelocity(state.speedMetersPerSecond / DrivebaseConstants.kWheelRadiusMeters); io.setTurnPosition(state.angle); } @@ -100,12 +100,12 @@ public Rotation2d getAngle() { /** Returns the current drive position of the module in meters. */ public double getPositionMeters() { - return inputs.drivePositionRad * kWheelRadiusMeters; + return inputs.drivePositionRad * DrivebaseConstants.kWheelRadiusMeters; } /** Returns the current drive velocity of the module in meters per second. */ public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * kWheelRadiusMeters; + return inputs.driveVelocityRadPerSec * DrivebaseConstants.kWheelRadiusMeters; } /** Returns the module position (turn angle and drive position). */ From 06654713ff63dc2279bb2b7afed3d988fe8882fd Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 25 Jan 2026 11:42:19 -0700 Subject: [PATCH 06/14] Compute max angular vel/accel in terms of max linear Instead of using arbitrary maximum angular velocity and acceleration, use the combination of the specified maximum linear limits and drivebase radius to compute the angular limits. --- src/main/java/frc/robot/Constants.java | 4 ---- .../java/frc/robot/subsystems/drive/Drive.java | 14 ++++++++++++-- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 82a79138..d7a382ee 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -262,9 +262,6 @@ public static final class DrivebaseConstants { // of YOUR ROBOT, and replace the estimate here with your measured value! public static final double kMaxLinearSpeed = Feet.of(18).in(Meters); - // Set 3/4 of a rotation per second as the max angular velocity (radians/sec) - public static final double kMaxAngularSpeed = 1.5 * Math.PI; - // Slip Current -- the current draw when the wheels start to slip // Measure this against a wall. CHECK WITH THE CARPET AT AN ACTUAL EVENT!!! public static final double kSlipCurrent = 20.0; // Amps @@ -275,7 +272,6 @@ public static final class DrivebaseConstants { // Maximum chassis accelerations desired for robot motion -- metric / radians // TODO: Compute the maximum linear acceleration given the PHYSICS of the ROBOT! public static final double kMaxLinearAccel = 4.0; // m/s/s - public static final double kMaxAngularAccel = Degrees.of(720).in(Radians); // For Profiled PID Motion -- NEED TUNING! // Used in a variety of contexts, including PathPlanner and AutoPilot diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 0a1435e8..286a8636 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -96,7 +96,7 @@ public Drive(ImuIO imuIO) { DrivebaseConstants.kISPin, DrivebaseConstants.kDSpin, new TrapezoidProfile.Constraints( - DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); + getMaxAngularSpeedRadPerSec(), getMaxLinearAccelMetersPerSecPerSec())); angleController.enableContinuousInput(-Math.PI, Math.PI); // If REAL (i.e., NOT simulation), parse out the module types @@ -368,7 +368,7 @@ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, Constants.loopPeriodSecs); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DrivebaseConstants.kMaxLinearSpeed); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, getMaxLinearSpeedMetersPerSec()); // Log unoptimized setpoints and setpoint speeds Logger.recordOutput("SwerveStates/Setpoints", setpointStates); @@ -527,6 +527,16 @@ public double getMaxAngularSpeedRadPerSec() { return getMaxLinearSpeedMetersPerSec() / kDriveBaseRadiusMeters; } + /** Returns the maximum linear acceleration in meters per sec per sec. */ + public double getMaxLinearAccelMetersPerSecPerSec() { + return DrivebaseConstants.kMaxLinearAccel; + } + + /** Returns the maximum angular acceleration in radians per sec per sec */ + public double getMaxAngularAccelRadPerSecPerSec() { + return getMaxLinearAccelMetersPerSecPerSec() / kDriveBaseRadiusMeters; + } + /* Setter Functions ****************************************************** */ /** Resets the current odometry pose. */ From ebaf435abc6dbe2cae6a8612e59a51aa8e9bf26f Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 25 Jan 2026 16:09:42 -0700 Subject: [PATCH 07/14] Update Flywheel gains in prep for MotionMagic --- src/main/java/frc/robot/Constants.java | 16 +++++---- .../subsystems/flywheel_example/Flywheel.java | 14 ++------ .../flywheel_example/FlywheelIO.java | 14 ++++---- .../flywheel_example/FlywheelIOSim.java | 20 +++++++++-- .../flywheel_example/FlywheelIOSpark.java | 26 ++++++++------ .../flywheel_example/FlywheelIOTalonFX.java | 34 +++++++++---------- 6 files changed, 69 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d7a382ee..92c4536e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -334,17 +334,21 @@ public static final class FlywheelConstants { // MODE == REAL / REPLAY // Feedforward constants - public static final double kStaticGainReal = 0.1; - public static final double kVelocityGainReal = 0.05; + public static final double kSreal = 0.1; + public static final double kVreal = 0.05; + public static final double kAreal = 0.0; // Feedback (PID) constants - public static final PIDConstants pidReal = new PIDConstants(1.0, 0.0, 0.0); + public static final double kPreal = 1.0; + public static final double kDreal = 0.0; // MODE == SIM // Feedforward constants - public static final double kStaticGainSim = 0.0; - public static final double kVelocityGainSim = 0.03; + public static final double kSsim = 0.0; + public static final double kVsim = 0.03; + public static final double kAsim = 0.0; // Feedback (PID) constants - public static final PIDConstants pidSim = new PIDConstants(1.0, 0.0, 0.0); + public static final double kPsim = 0.0; + public static final double kDsim = 0.0; } /************************************************************************* */ 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 8b934be3..80406e50 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -12,7 +12,6 @@ import static edu.wpi.first.units.Units.Volts; import static frc.robot.Constants.FlywheelConstants.*; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -24,7 +23,6 @@ public class Flywheel extends RBSISubsystem { private final FlywheelIO io; private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); - private final SimpleMotorFeedforward ffModel; private final SysIdRoutine sysId; /** Creates a new Flywheel. */ @@ -36,17 +34,11 @@ public Flywheel(FlywheelIO io) { switch (Constants.getMode()) { case REAL: case REPLAY: - ffModel = new SimpleMotorFeedforward(kStaticGainReal, kVelocityGainReal); - io.configurePID(pidReal.kP, pidReal.kI, pidReal.kD); - io.configureFF(kStaticGainReal, kVelocityGainReal); + io.configureGains(kPreal, 0.0, kDreal, kSreal, kVreal, kAreal); break; case SIM: - ffModel = new SimpleMotorFeedforward(kStaticGainSim, kVelocityGainSim); - io.configurePID(pidSim.kP, pidSim.kI, pidSim.kD); - io.configureFF(kStaticGainSim, kVelocityGainSim); - break; default: - ffModel = new SimpleMotorFeedforward(0.0, 0.0); + io.configureGains(kPsim, 0.0, kDsim, kSsim, kVsim, kAsim); break; } @@ -76,7 +68,7 @@ public void runVolts(double volts) { /** Run closed loop at the specified velocity. */ public void runVelocity(double velocityRPM) { var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); + io.setVelocity(velocityRadPerSec); // Log flywheel setpoint Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM); 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 c1b6901f..47fa8748 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java @@ -26,14 +26,12 @@ public static class FlywheelIOInputs { public default void updateInputs(FlywheelIOInputs inputs) {} /** Run closed loop at the specified velocity. */ - public default void setVelocity(double velocityRadPerSec, double ffVolts) {} + public default void setVelocity(double velocityRadPerSec) {} - /** Set velocity PID constants. */ - public default void configurePID(double kP, double kI, double kD) {} + /** Set gain constants */ + public default void configureGains(double kP, double kI, double kD, double kS, double kV) {} - /** Set velocity FeedForward constants. */ - public default void configureFF(double kS, double kV) {} - - /** Set velocity FeedForward constants. */ - public default void configureFF(double kS, double kV, double kA) {} + /** Set gain constants */ + public default void configureGains( + double kP, double kI, double kD, double kS, double kV, double kA) {} } 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 d65eade1..a8e8c20f 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; @@ -33,6 +34,7 @@ public class FlywheelIOSim implements FlywheelIO { private final FlywheelSim sim = new FlywheelSim(m_plant, m_gearbox); private PIDController pid = new PIDController(0.0, 0.0, 0.0); + private SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.0, 0.0, 0.0); private boolean closedLoop = false; private double ffVolts = 0.0; @@ -62,10 +64,10 @@ public void setVoltage(double volts) { } @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { + public void setVelocity(double velocityRadPerSec) { closedLoop = true; pid.setSetpoint(velocityRadPerSec); - this.ffVolts = ffVolts; + this.ffVolts = ff.calculate(velocityRadPerSec); } @Override @@ -73,8 +75,20 @@ public void stop() { setVoltage(0.0); } + /** Set gain constants -- no kA */ @Override - public void configurePID(double kP, double kI, double kD) { + public void configureGains(double kP, double kI, double kD, double kS, double kV) { pid.setPID(kP, kI, kD); + ff.setKs(kS); + ff.setKv(kV); + ff.setKa(0.0); + } + + /** Set gain constants - with kA */ + public void configureGains(double kP, double kI, double kD, double kS, double kV, double kA) { + pid.setPID(kP, kI, kD); + ff.setKs(kS); + ff.setKv(kV); + ff.setKa(kA); } } 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 cef38ced..ee89859e 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -30,6 +30,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import frc.robot.Constants; import frc.robot.Constants.DrivebaseConstants; @@ -54,6 +55,7 @@ public class FlywheelIOSpark implements FlywheelIO { public final int[] powerPorts = { FLYWHEEL_LEADER.getPowerPort(), FLYWHEEL_FOLLOWER.getPowerPort() }; + private final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kSreal, kVreal, kAreal); public FlywheelIOSpark() { @@ -71,10 +73,11 @@ public FlywheelIOSpark() { leaderConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(pidReal.kP, pidReal.kI, pidReal.kD) + .pid(kPreal, 0.0, kDreal) .feedForward - .kS(kStaticGainReal) - .kV(kVelocityGainReal); + .kS(kSreal) + .kV(kVreal) + .kA(kAreal); leaderConfig .signals .primaryEncoderPositionAlwaysOn(true) @@ -118,7 +121,8 @@ public void setVoltage(double volts) { } @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { + public void setVelocity(double velocityRadPerSec) { + double ffVolts = ff.calculate(velocityRadPerSec); pid.setSetpoint( Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * kFlywheelGearRatio, ControlType.kVelocity, @@ -133,14 +137,14 @@ public void stop() { } /** - * Configure the closed-loop PID + * 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. */ @Override - public void configurePID(double kP, double kI, double kD) { + 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); @@ -148,8 +152,10 @@ public void configurePID(double kP, double kI, double kD) { } @Override - public void configureFF(double kS, double kV) {} - - @Override - public void configureFF(double kS, double kV, double kA) {} + 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); + } } 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 04391d6d..99dfb2cb 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -28,6 +28,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.PowerConstants; import frc.robot.util.PhoenixUtil; public class FlywheelIOTalonFX implements FlywheelIO { @@ -51,7 +52,7 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final TalonFXConfiguration config = new TalonFXConfiguration(); public FlywheelIOTalonFX() { - config.CurrentLimits.SupplyCurrentLimit = 30.0; + config.CurrentLimits.SupplyCurrentLimit = PowerConstants.kMotorPortMaxCurrent; config.CurrentLimits.SupplyCurrentLimitEnable = true; config.MotorOutput.NeutralMode = switch (kFlywheelIdleMode) { @@ -101,7 +102,7 @@ public void setVoltage(double volts) { } @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { + public void setVelocity(double velocityRadPerSec) { leader.setControl(new VelocityVoltage(Units.radiansToRotations(velocityRadPerSec))); } @@ -111,41 +112,40 @@ public void stop() { } /** - * Set the PID portion of the Slot0 closed-loop configuration + * Set the gains of the Slot0 closed-loop configuration * * @param kP Proportional gain * @param kI Integral gain * @param kD Differential gain + * @param kS Static gain + * @param kV Velocity gain */ @Override - public void configurePID(double kP, double kI, double kD) { + public void configureGains(double kP, double kI, double kD, double kS, double kV) { config.Slot0.kP = kP; config.Slot0.kI = kI; config.Slot0.kD = kD; - PhoenixUtil.tryUntilOk(5, () -> leader.getConfigurator().apply(config, 0.25)); - } - - /** - * Set the FeedForward portion of the Slot0 closed-loop configuration - * - * @param kS Static gain - * @param kV Velocity gain - */ - @Override - public void configureFF(double kS, double kV) { config.Slot0.kS = kS; config.Slot0.kV = kV; + config.Slot0.kA = 0.0; PhoenixUtil.tryUntilOk(5, () -> leader.getConfigurator().apply(config, 0.25)); } /** - * Set the FeedForward portion of the Slot0 closed-loop configuration + * Set the gains of the Slot0 closed-loop configuration * + * @param kP Proportional gain + * @param kI Integral gain + * @param kD Differential gain * @param kS Static gain * @param kV Velocity gain * @param kA Acceleration gain */ - public void configureFF(double kS, double kV, double kA) { + @Override + public void configureGains(double kP, double kI, double kD, double kS, double kV, double kA) { + config.Slot0.kP = kP; + config.Slot0.kI = kI; + config.Slot0.kD = kD; config.Slot0.kS = kS; config.Slot0.kV = kV; config.Slot0.kA = kA; From 15bb39f824522dc757e198cb1c3846dab79f5261 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Sun, 25 Jan 2026 18:21:47 -0700 Subject: [PATCH 08/14] Add CTRE MotionMagic to Flywheel Example --- .../flywheel_example/FlywheelIOTalonFX.java | 34 +++++++++++++++---- 1 file changed, 28 insertions(+), 6 deletions(-) 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 99dfb2cb..1b964f5c 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -18,8 +18,9 @@ import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -28,8 +29,10 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; import frc.robot.Constants.PowerConstants; import frc.robot.util.PhoenixUtil; +import frc.robot.util.RBSIEnum.CTREPro; public class FlywheelIOTalonFX implements FlywheelIO { @@ -50,6 +53,7 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final StatusSignal followerCurrent = follower.getSupplyCurrent(); private final TalonFXConfiguration config = new TalonFXConfiguration(); + private final boolean isCTREPro = Constants.getPhoenixPro() == CTREPro.LICENSED; public FlywheelIOTalonFX() { config.CurrentLimits.SupplyCurrentLimit = PowerConstants.kMotorPortMaxCurrent; @@ -70,10 +74,15 @@ public FlywheelIOTalonFX() { closedRamps.TorqueClosedLoopRampPeriod = kFlywheelClosedLoopRampPeriod; // 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) // Apply the configurations to the flywheel motors - leader.getConfigurator().apply(config); - follower.getConfigurator().apply(config); + PhoenixUtil.tryUntilOk(5, () -> leader.getConfigurator().apply(config, 0.25)); + PhoenixUtil.tryUntilOk(5, () -> follower.getConfigurator().apply(config, 0.25)); // If follower rotates in the opposite direction, set "MotorAlignmentValue" to Opposed follower.setControl(new Follower(leader.getDeviceID(), MotorAlignmentValue.Aligned)); @@ -98,12 +107,25 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setVoltage(double volts) { - leader.setControl(new VoltageOut(volts)); + final MotionMagicVoltage m_request = new MotionMagicVoltage(volts); + m_request.withEnableFOC(isCTREPro); + leader.setControl(m_request); } @Override public void setVelocity(double velocityRadPerSec) { - leader.setControl(new VelocityVoltage(Units.radiansToRotations(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))); + } + + @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); } @Override From 3e31af17a51a92f2c79fa5714cd6b803575e2da7 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Mon, 26 Jan 2026 21:27:13 -0700 Subject: [PATCH 09/14] Allow `driveStyle` to be choosable from the Dashbaord In addition to the compile-and-build setting of drive style between TANK and GAMER, allow such to be set via a Dashboard chooser. In this way, the setting can be changed without recompile and redeploy between matches, if desired. --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8bb188e4..531c4eeb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,6 +63,7 @@ import frc.robot.util.LoggedTunableNumber; import frc.robot.util.OverrideSwitches; import frc.robot.util.RBSIEnum.AutoType; +import frc.robot.util.RBSIEnum.DriveStyle; import frc.robot.util.RBSIEnum.Mode; import java.util.Set; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -107,6 +108,9 @@ public class RobotContainer { // AutoChoosers for both supported path planning types private final LoggedDashboardChooser autoChooserPathPlanner; + private final LoggedDashboardChooser driveStyle = + new LoggedDashboardChooser<>("Drive Style"); + private final AutoChooser autoChooserChoreo; private final AutoFactory autoFactoryChoreo; // Input estimated battery capacity (if full, use printed value) @@ -259,6 +263,10 @@ public RobotContainer() { "Incorrect AUTO type selected in Constants: " + Constants.getAutoType()); } + // Get drive style from the Dashboard Chooser + driveStyle.addDefaultOption("TANK", DriveStyle.TANK); + driveStyle.addOption("GAMER", DriveStyle.GAMER); + // Define Auto commands defineAutoCommands(); // Define SysIs Routines @@ -285,6 +293,8 @@ private void configureBindings() { GetJoystickValue driveStickY; GetJoystickValue driveStickX; GetJoystickValue turnStickX; + // OPTIONAL: Use the DashboardChooser rather than the Constants file for Drive Style + // switch (driveStyle.get()) { switch (OperatorConstants.kDriveStyle) { case GAMER: driveStickY = driverController::getRightY; From 3fa56edcaa11382d77b98e9d34a6a8002d2b239d Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 28 Jan 2026 08:35:13 -0700 Subject: [PATCH 10/14] Cleaning CANBus declarations; streamlining periodics Make a clear place for defining which CANBus a device is running on, and propagate that throughout. Also, streamline what goes on inside some periodic() functions to minimize load and garbage collection. --- src/main/java/frc/robot/Constants.java | 53 ++++++++- src/main/java/frc/robot/Robot.java | 12 +- .../accelerometer/Accelerometer.java | 2 +- .../subsystems/drive/ModuleIOBlended.java | 107 +++++++++--------- .../robot/subsystems/drive/ModuleIOSpark.java | 63 ++++++----- .../drive/ModuleIOSparkCANcoder.java | 74 ++++++------ .../subsystems/drive/ModuleIOTalonFX.java | 10 +- .../subsystems/drive/ModuleIOTalonFXS.java | 10 +- .../subsystems/drive/SwerveConstants.java | 30 +++-- .../robot/subsystems/imu/ImuIOPigeon2.java | 4 +- .../frc/robot/subsystems/vision/Vision.java | 46 ++++---- .../vision/VisionIOPhotonVision.java | 23 +++- .../java/frc/robot/util/RBSIPowerMonitor.java | 22 ++-- .../java/frc/robot/util/RBSISubsystem.java | 3 +- .../java/frc/robot/util/RobotDeviceId.java | 1 + .../java/frc/robot/util/VirtualSubsystem.java | 24 +++- 16 files changed, 289 insertions(+), 195 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 92c4536e..5d4111ca 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,6 +19,7 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; @@ -45,6 +46,9 @@ import frc.robot.util.RBSIEnum.SwerveType; import frc.robot.util.RBSIEnum.VisionType; import frc.robot.util.RobotDeviceId; +import java.util.Collections; +import java.util.HashMap; +import java.util.Map; import org.photonvision.simulation.SimCameraProperties; import swervelib.math.Matter; @@ -161,13 +165,56 @@ public static final class PowerConstants { public static final double kVoltageCritical = 6.5; } + /************************************************************************* */ + /** List of Robot CAN Busses ********************************************* */ + public static final class CANBuses { + + // ---- Bus names (single source of truth) ---- + public static final String DRIVE = "DriveTrain"; + public static final String RIO = ""; + // In 2027 and later, you'll be able to have even more CAN Busses! + + // ---- Singleton instances (exactly one per bus) ---- + public static final CANBus DRIVE_BUS = new CANBus(DRIVE); + public static final CANBus RIO_BUS = new CANBus(RIO); + + // ---- Lookup table: name -> CANBus singleton ---- + private static final Map BY_NAME; + + static { + Map m = new HashMap<>(); + m.put(DRIVE, DRIVE_BUS); + m.put(RIO, RIO_BUS); + BY_NAME = Collections.unmodifiableMap(m); + } + + /** + * Get the singleton CANBus for a given bus name. + * + *

Usage: CANBus bus = Constants.CANBuses.get(Constants.CANBuses.DRIVE); + */ + public static CANBus get(String name) { + CANBus bus = BY_NAME.get(name); + if (bus == null) { + throw new IllegalArgumentException( + "Unknown CAN bus name '" + name + "'. Known buses: " + BY_NAME.keySet()); + } + return bus; + } + + /** Expose known bus names for debugging. */ + public static Map all() { + return BY_NAME; + } + } + /************************************************************************* */ /** List of Robot Device CAN and Power Distribution Circuit IDs ********** */ public static class RobotDevices { /* DRIVETRAIN CAN DEVICE IDS */ // Input the correct Power Distribution Module port for each motor!!!! - // NOTE: The CAN ID and bus are set in the Swerve Generator (Phoenix Tuner or YAGSL) + // NOTE: The CAN ID's are set in the Swerve Generator (Phoenix Tuner or YAGSL) // Front Left public static final RobotDeviceId FL_DRIVE = @@ -204,8 +251,8 @@ public static class RobotDevices { /* SUBSYSTEM CAN DEVICE IDS */ // This is where mechanism subsystem devices are defined (Including ID, bus, and power port) // Example: - public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, "", 8); - public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, "", 9); + public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, CANBuses.RIO, 8); + public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, CANBuses.RIO, 9); /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ // This is where digital I/O feedback devices are defined diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0ebfbe54..d39bafa1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -112,15 +112,16 @@ public Robot() { // Create a timer to disable motor brake a few seconds after disable. This will let the robot // stop immediately when disabled, but then also let it be pushed more m_disabledTimer = new Timer(); - } - /** This function is called periodically during all modes. */ - @Override - public void robotPeriodic() { // Switch thread to high priority to improve loop timing if (isReal()) { Threads.setCurrentThreadPriority(true, 99); } + } + + /** This function is called periodically during all modes. */ + @Override + public void robotPeriodic() { // Run all virtual subsystems each time through the loop VirtualSubsystem.periodicAll(); @@ -131,9 +132,6 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); - - // Return to normal thread priority - Threads.setCurrentThreadPriority(false, 10); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 7039b86b..4629b6a0 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -44,7 +44,7 @@ public Accelerometer(ImuIO imuIO) { } @Override - public void periodic() { + public void rbsiPeriodic() { // --- Update IMU readings --- imuIO.updateInputs(imuInputs); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index d9029480..6319902c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -10,9 +10,9 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; @@ -52,6 +52,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; +import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.PhoenixUtil; import frc.robot.util.SparkUtil; @@ -137,54 +138,55 @@ public ModuleIOBlended(int module) { switch (module) { case 0 -> ConstantCreator.createModuleConstants( - kFLSteerMotorId, - kFLDriveMotorId, - kFLEncoderId, - kFLEncoderOffset, - kFLXPosMeters, - kFLYPosMeters, - kFLDriveInvert, - kFLSteerInvert, - kFLEncoderInvert); + SwerveConstants.kFLSteerMotorId, + SwerveConstants.kFLDriveMotorId, + SwerveConstants.kFLEncoderId, + SwerveConstants.kFLEncoderOffset, + SwerveConstants.kFLXPosMeters, + SwerveConstants.kFLYPosMeters, + SwerveConstants.kFLDriveInvert, + SwerveConstants.kFLSteerInvert, + SwerveConstants.kFLEncoderInvert); case 1 -> ConstantCreator.createModuleConstants( - kFRSteerMotorId, - kFRDriveMotorId, - kFREncoderId, - kFREncoderOffset, - kFRXPosMeters, - kFRYPosMeters, - kFRDriveInvert, - kFRSteerInvert, - kFREncoderInvert); + SwerveConstants.kFRSteerMotorId, + SwerveConstants.kFRDriveMotorId, + SwerveConstants.kFREncoderId, + SwerveConstants.kFREncoderOffset, + SwerveConstants.kFRXPosMeters, + SwerveConstants.kFRYPosMeters, + SwerveConstants.kFRDriveInvert, + SwerveConstants.kFRSteerInvert, + SwerveConstants.kFREncoderInvert); case 2 -> ConstantCreator.createModuleConstants( - kBLSteerMotorId, - kBLDriveMotorId, - kBLEncoderId, - kBLEncoderOffset, - kBLXPosMeters, - kBLYPosMeters, - kBLDriveInvert, - kBLSteerInvert, - kBLEncoderInvert); + SwerveConstants.kBLSteerMotorId, + SwerveConstants.kBLDriveMotorId, + SwerveConstants.kBLEncoderId, + SwerveConstants.kBLEncoderOffset, + SwerveConstants.kBLXPosMeters, + SwerveConstants.kBLYPosMeters, + SwerveConstants.kBLDriveInvert, + SwerveConstants.kBLSteerInvert, + SwerveConstants.kBLEncoderInvert); case 3 -> ConstantCreator.createModuleConstants( - kBRSteerMotorId, - kBRDriveMotorId, - kBREncoderId, - kBREncoderOffset, - kBRXPosMeters, - kBRYPosMeters, - kBRDriveInvert, - kBRSteerInvert, - kBREncoderInvert); + SwerveConstants.kBRSteerMotorId, + SwerveConstants.kBRDriveMotorId, + SwerveConstants.kBREncoderId, + SwerveConstants.kBREncoderOffset, + SwerveConstants.kBRXPosMeters, + SwerveConstants.kBRYPosMeters, + SwerveConstants.kBRDriveInvert, + SwerveConstants.kBRSteerInvert, + SwerveConstants.kBREncoderInvert); default -> throw new IllegalArgumentException("Invalid module index"); }; - driveTalon = new TalonFX(constants.DriveMotorId, kCANBus); + CANBus canBus = CANBuses.get(SwerveConstants.kCANbusName); + driveTalon = new TalonFX(constants.DriveMotorId, canBus); turnSpark = new SparkMax(constants.SteerMotorId, MotorType.kBrushless); - cancoder = new CANcoder(constants.EncoderId, kCANBus); + cancoder = new CANcoder(constants.EncoderId, canBus); turnController = turnSpark.getClosedLoopController(); @@ -229,21 +231,22 @@ public ModuleIOBlended(int module) { turnConfig .absoluteEncoder .inverted(constants.EncoderInverted) - .positionConversionFactor(turnEncoderPositionFactor) - .velocityConversionFactor(turnEncoderVelocityFactor) + .positionConversionFactor(SwerveConstants.turnEncoderPositionFactor) + .velocityConversionFactor(SwerveConstants.turnEncoderVelocityFactor) .averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .positionWrappingInputRange( + SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) .feedForward .kV(0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) @@ -434,8 +437,8 @@ public void setTurnPosition(Rotation2d rotation) { double setpoint = MathUtil.inputModulus( rotation.plus(Rotation2d.fromRotations(constants.EncoderOffset)).getRadians(), - turnPIDMinInput, - turnPIDMaxInput); + SwerveConstants.turnPIDMinInput, + SwerveConstants.turnPIDMaxInput); turnController.setSetpoint(setpoint, ControlType.kPosition); } @@ -444,12 +447,12 @@ public void setTurnPosition(Rotation2d rotation) { ConstantCreator = new SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) + .withDriveMotorGearRatio(SwerveConstants.kDriveGearRatio) + .withSteerMotorGearRatio(SwerveConstants.kSteerGearRatio) + .withCouplingGearRatio(SwerveConstants.kCoupleRatio) .withWheelRadius(DrivebaseConstants.kWheelRadiusMeters) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + .withSteerInertia(SwerveConstants.kSteerInertia) + .withDriveInertia(SwerveConstants.kDriveInertia) + .withSteerFrictionVoltage(SwerveConstants.kSteerFrictionVoltage) + .withDriveFrictionVoltage(SwerveConstants.kDriveFrictionVoltage); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index c2fdc354..75738618 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -9,8 +9,6 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.SwerveConstants.*; - import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; @@ -78,46 +76,46 @@ public class ModuleIOSpark implements ModuleIO { public ModuleIOSpark(int module) { zeroRotation = switch (module) { - case 0 -> new Rotation2d(kFLEncoderOffset); - case 1 -> new Rotation2d(kFREncoderOffset); - case 2 -> new Rotation2d(kBLEncoderOffset); - case 3 -> new Rotation2d(kBREncoderOffset); + case 0 -> new Rotation2d(SwerveConstants.kFLEncoderOffset); + case 1 -> new Rotation2d(SwerveConstants.kFREncoderOffset); + case 2 -> new Rotation2d(SwerveConstants.kBLEncoderOffset); + case 3 -> new Rotation2d(SwerveConstants.kBREncoderOffset); default -> Rotation2d.kZero; }; driveSpark = new SparkFlex( switch (module) { - case 0 -> kFLDriveMotorId; - case 1 -> kFRDriveMotorId; - case 2 -> kBLDriveMotorId; - case 3 -> kBRDriveMotorId; + case 0 -> SwerveConstants.kFLDriveMotorId; + case 1 -> SwerveConstants.kFRDriveMotorId; + case 2 -> SwerveConstants.kBLDriveMotorId; + case 3 -> SwerveConstants.kBRDriveMotorId; default -> 0; }, MotorType.kBrushless); turnSpark = new SparkMax( switch (module) { - case 0 -> kFLSteerMotorId; - case 1 -> kFRSteerMotorId; - case 2 -> kBLSteerMotorId; - case 3 -> kBRSteerMotorId; + case 0 -> SwerveConstants.kFLSteerMotorId; + case 1 -> SwerveConstants.kFRSteerMotorId; + case 2 -> SwerveConstants.kBLSteerMotorId; + case 3 -> SwerveConstants.kBRSteerMotorId; default -> 0; }, MotorType.kBrushless); turnInverted = switch (module) { - case 0 -> kFLSteerInvert; - case 1 -> kFRSteerInvert; - case 2 -> kBLSteerInvert; - case 3 -> kBRSteerInvert; + case 0 -> SwerveConstants.kFLSteerInvert; + case 1 -> SwerveConstants.kFRSteerInvert; + case 2 -> SwerveConstants.kBLSteerInvert; + case 3 -> SwerveConstants.kBRSteerInvert; default -> false; }; turnEncoderInverted = switch (module) { - case 0 -> kFLEncoderInvert; - case 1 -> kFREncoderInvert; - case 2 -> kBLEncoderInvert; - case 3 -> kBREncoderInvert; + case 0 -> SwerveConstants.kFLEncoderInvert; + case 1 -> SwerveConstants.kFREncoderInvert; + case 2 -> SwerveConstants.kBLEncoderInvert; + case 3 -> SwerveConstants.kBREncoderInvert; default -> false; }; driveEncoder = driveSpark.getEncoder(); @@ -129,12 +127,12 @@ public ModuleIOSpark(int module) { var driveConfig = new SparkFlexConfig(); driveConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit((int) kDriveCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) .voltageCompensation(DrivebaseConstants.kOptimalVoltage); driveConfig .encoder - .positionConversionFactor(driveEncoderPositionFactor) - .velocityConversionFactor(driveEncoderVelocityFactor) + .positionConversionFactor(SwerveConstants.driveEncoderPositionFactor) + .velocityConversionFactor(SwerveConstants.driveEncoderVelocityFactor) .uvwMeasurementPeriod(10) .uvwAverageDepth(2); driveConfig @@ -147,7 +145,7 @@ public ModuleIOSpark(int module) { driveConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) @@ -174,21 +172,22 @@ public ModuleIOSpark(int module) { turnConfig .absoluteEncoder .inverted(turnEncoderInverted) - .positionConversionFactor(turnEncoderPositionFactor) - .velocityConversionFactor(turnEncoderVelocityFactor) + .positionConversionFactor(SwerveConstants.turnEncoderPositionFactor) + .velocityConversionFactor(SwerveConstants.turnEncoderVelocityFactor) .averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .positionWrappingInputRange( + SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) .feedForward .kV(0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) @@ -332,7 +331,9 @@ public void setDriveVelocity(double velocityRadPerSec) { public void setTurnPosition(Rotation2d rotation) { double setpoint = MathUtil.inputModulus( - rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); + rotation.plus(zeroRotation).getRadians(), + SwerveConstants.turnPIDMinInput, + SwerveConstants.turnPIDMaxInput); turnController.setSetpoint(setpoint, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index 8f47a65b..73270a7f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -9,8 +9,6 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.SwerveConstants.*; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.CANcoder; @@ -37,6 +35,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; +import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.SparkUtil; import java.util.Queue; @@ -90,59 +89,59 @@ public class ModuleIOSparkCANcoder implements ModuleIO { public ModuleIOSparkCANcoder(int module) { zeroRotation = switch (module) { - case 0 -> new Rotation2d(kFLEncoderOffset); - case 1 -> new Rotation2d(kFREncoderOffset); - case 2 -> new Rotation2d(kBLEncoderOffset); - case 3 -> new Rotation2d(kBREncoderOffset); + case 0 -> new Rotation2d(SwerveConstants.kFLEncoderOffset); + case 1 -> new Rotation2d(SwerveConstants.kFREncoderOffset); + case 2 -> new Rotation2d(SwerveConstants.kBLEncoderOffset); + case 3 -> new Rotation2d(SwerveConstants.kBREncoderOffset); default -> Rotation2d.kZero; }; driveSpark = new SparkFlex( switch (module) { - case 0 -> kFLDriveMotorId; - case 1 -> kFRDriveMotorId; - case 2 -> kBLDriveMotorId; - case 3 -> kBRDriveMotorId; + case 0 -> SwerveConstants.kFLDriveMotorId; + case 1 -> SwerveConstants.kFRDriveMotorId; + case 2 -> SwerveConstants.kBLDriveMotorId; + case 3 -> SwerveConstants.kBRDriveMotorId; default -> 0; }, MotorType.kBrushless); turnSpark = new SparkMax( switch (module) { - case 0 -> kFLSteerMotorId; - case 1 -> kFRSteerMotorId; - case 2 -> kBLSteerMotorId; - case 3 -> kBRSteerMotorId; + case 0 -> SwerveConstants.kFLSteerMotorId; + case 1 -> SwerveConstants.kFRSteerMotorId; + case 2 -> SwerveConstants.kBLSteerMotorId; + case 3 -> SwerveConstants.kBRSteerMotorId; default -> 0; }, MotorType.kBrushless); turnInverted = switch (module) { - case 0 -> kFLSteerInvert; - case 1 -> kFRSteerInvert; - case 2 -> kBLSteerInvert; - case 3 -> kBRSteerInvert; + case 0 -> SwerveConstants.kFLSteerInvert; + case 1 -> SwerveConstants.kFRSteerInvert; + case 2 -> SwerveConstants.kBLSteerInvert; + case 3 -> SwerveConstants.kBRSteerInvert; default -> false; }; turnEncoderInverted = switch (module) { - case 0 -> kFLEncoderInvert; - case 1 -> kFREncoderInvert; - case 2 -> kBLEncoderInvert; - case 3 -> kBREncoderInvert; + case 0 -> SwerveConstants.kFLEncoderInvert; + case 1 -> SwerveConstants.kFREncoderInvert; + case 2 -> SwerveConstants.kBLEncoderInvert; + case 3 -> SwerveConstants.kBREncoderInvert; default -> false; }; driveEncoder = driveSpark.getEncoder(); cancoder = new CANcoder( switch (module) { - case 0 -> kFLEncoderId; - case 1 -> kFREncoderId; - case 2 -> kBLEncoderId; - case 3 -> kBREncoderId; + case 0 -> SwerveConstants.kFLEncoderId; + case 1 -> SwerveConstants.kFREncoderId; + case 2 -> SwerveConstants.kBLEncoderId; + case 3 -> SwerveConstants.kBREncoderId; default -> 0; }, - kCANBus); + CANBuses.get(SwerveConstants.kCANbusName)); driveController = driveSpark.getClosedLoopController(); turnController = turnSpark.getClosedLoopController(); @@ -150,12 +149,12 @@ public ModuleIOSparkCANcoder(int module) { var driveConfig = new SparkFlexConfig(); driveConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit((int) kDriveCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) .voltageCompensation(DrivebaseConstants.kOptimalVoltage); driveConfig .encoder - .positionConversionFactor(driveEncoderPositionFactor) - .velocityConversionFactor(driveEncoderVelocityFactor) + .positionConversionFactor(SwerveConstants.driveEncoderPositionFactor) + .velocityConversionFactor(SwerveConstants.driveEncoderVelocityFactor) .uvwMeasurementPeriod(10) .uvwAverageDepth(2); driveConfig @@ -168,7 +167,7 @@ public ModuleIOSparkCANcoder(int module) { driveConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -192,21 +191,22 @@ public ModuleIOSparkCANcoder(int module) { turnConfig .absoluteEncoder .inverted(turnEncoderInverted) - .positionConversionFactor(turnEncoderPositionFactor) - .velocityConversionFactor(turnEncoderVelocityFactor) + .positionConversionFactor(SwerveConstants.turnEncoderPositionFactor) + .velocityConversionFactor(SwerveConstants.turnEncoderVelocityFactor) .averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .positionWrappingInputRange( + SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) .feedForward .kV(0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) @@ -362,7 +362,9 @@ public void setDriveVelocity(double velocityRadPerSec) { public void setTurnPosition(Rotation2d rotation) { double setpoint = MathUtil.inputModulus( - rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); + rotation.plus(zeroRotation).getRadians(), + SwerveConstants.turnPIDMinInput, + SwerveConstants.turnPIDMaxInput); turnController.setSetpoint(setpoint, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index b9614d20..b616b0be 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -10,9 +10,9 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; @@ -44,6 +44,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; +import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.generated.TunerConstants; import frc.robot.util.PhoenixUtil; @@ -135,9 +136,10 @@ public ModuleIOTalonFX(int module) { default -> throw new IllegalArgumentException("Invalid module index"); }; - driveTalon = new TalonFX(constants.DriveMotorId, kCANBus); - turnTalon = new TalonFX(constants.SteerMotorId, kCANBus); - cancoder = new CANcoder(constants.EncoderId, kCANBus); + CANBus canBus = CANBuses.get(SwerveConstants.kCANbusName); + driveTalon = new TalonFX(constants.DriveMotorId, canBus); + turnTalon = new TalonFX(constants.SteerMotorId, canBus); + cancoder = new CANcoder(constants.EncoderId, canBus); // Configure drive motor driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index 5a809976..34bdf72c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -10,9 +10,9 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANdiConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; @@ -45,6 +45,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; +import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.PhoenixUtil; import java.util.Queue; @@ -124,9 +125,10 @@ public class ModuleIOTalonFXS implements ModuleIO { public ModuleIOTalonFXS( SwerveModuleConstants constants) { - driveTalon = new TalonFXS(constants.DriveMotorId, kCANBus); - turnTalon = new TalonFXS(constants.SteerMotorId, kCANBus); - candi = new CANdi(constants.EncoderId, kCANBus); + CANBus canBus = CANBuses.get(SwerveConstants.kCANbusName); + driveTalon = new TalonFXS(constants.DriveMotorId, canBus); + turnTalon = new TalonFXS(constants.SteerMotorId, canBus); + candi = new CANdi(constants.EncoderId, canBus); // Configure drive motor driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index 57e89d51..98f775fa 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.CANBus; import edu.wpi.first.math.util.Units; import frc.robot.Constants; +import frc.robot.Constants.CANBuses; import frc.robot.generated.TunerConstants; import frc.robot.util.YagslConstants; @@ -33,7 +34,6 @@ public class SwerveConstants { public static final double kDriveGearRatio; public static final double kSteerGearRatio; public static final String kCANbusName; - public static final CANBus kCANBus; public static final int kPigeonId; public static final double kSteerInertia; public static final double kDriveInertia; @@ -110,8 +110,7 @@ public class SwerveConstants { kCoupleRatio = TunerConstants.FrontLeft.CouplingGearRatio; kDriveGearRatio = TunerConstants.FrontLeft.DriveMotorGearRatio; kSteerGearRatio = TunerConstants.FrontLeft.SteerMotorGearRatio; - kCANbusName = TunerConstants.DrivetrainConstants.CANBusName; - kCANBus = new CANBus(TunerConstants.DrivetrainConstants.CANBusName); + kCANbusName = CANBuses.DRIVE; kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id; kSteerInertia = TunerConstants.FrontLeft.SteerInertia; kDriveInertia = TunerConstants.FrontLeft.DriveInertia; @@ -123,9 +122,9 @@ public class SwerveConstants { kFLDriveMotorId = TunerConstants.FrontLeft.DriveMotorId; kFLSteerMotorId = TunerConstants.FrontLeft.SteerMotorId; kFLEncoderId = TunerConstants.FrontLeft.EncoderId; - kFLDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFLSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFLEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFLDriveCanbus = kCANbusName; + kFLSteerCanbus = kCANbusName; + kFLEncoderCanbus = kCANbusName; kFLDriveType = "kraken"; kFLSteerType = "kraken"; kFLEncoderType = "cancoder"; @@ -140,9 +139,9 @@ public class SwerveConstants { kFRDriveMotorId = TunerConstants.FrontRight.DriveMotorId; kFRSteerMotorId = TunerConstants.FrontRight.SteerMotorId; kFREncoderId = TunerConstants.FrontRight.EncoderId; - kFRDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFRSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFREncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFRDriveCanbus = kCANbusName; + kFRSteerCanbus = kCANbusName; + kFREncoderCanbus = kCANbusName; kFRDriveType = "kraken"; kFRSteerType = "kraken"; kFREncoderType = "cancoder"; @@ -156,9 +155,9 @@ public class SwerveConstants { kBLDriveMotorId = TunerConstants.BackLeft.DriveMotorId; kBLSteerMotorId = TunerConstants.BackLeft.SteerMotorId; kBLEncoderId = TunerConstants.BackLeft.EncoderId; - kBLDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBLSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBLEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBLDriveCanbus = kCANbusName; + kBLSteerCanbus = kCANbusName; + kBLEncoderCanbus = kCANbusName; kBLDriveType = "kraken"; kBLSteerType = "kraken"; kBLEncoderType = "cancoder"; @@ -173,9 +172,9 @@ public class SwerveConstants { kBRDriveMotorId = TunerConstants.BackRight.DriveMotorId; kBRSteerMotorId = TunerConstants.BackRight.SteerMotorId; kBREncoderId = TunerConstants.BackRight.EncoderId; - kBRDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBRSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBREncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBRDriveCanbus = kCANbusName; + kBRSteerCanbus = kCANbusName; + kBREncoderCanbus = kCANbusName; kBRDriveType = "kraken"; kBRSteerType = "kraken"; kBREncoderType = "cancoder"; @@ -193,7 +192,6 @@ public class SwerveConstants { kDriveGearRatio = YagslConstants.kDriveGearRatio; kSteerGearRatio = YagslConstants.kSteerGearRatio; kCANbusName = YagslConstants.kCANbusName; - kCANBus = new CANBus(YagslConstants.kCANbusName); kPigeonId = YagslConstants.kPigeonId; kSteerInertia = YagslConstants.kSteerInertia; kDriveInertia = YagslConstants.kDriveInertia; diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java index d954f6b4..664dafb9 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java @@ -28,6 +28,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants.CANBuses; import frc.robot.subsystems.drive.PhoenixOdometryThread; import frc.robot.subsystems.drive.SwerveConstants; import java.util.Queue; @@ -35,7 +36,8 @@ /** IMU IO for CTRE Pigeon2 */ public class ImuIOPigeon2 implements ImuIO { - private final Pigeon2 pigeon = new Pigeon2(SwerveConstants.kPigeonId, SwerveConstants.kCANBus); + private final Pigeon2 pigeon = + new Pigeon2(SwerveConstants.kPigeonId, CANBuses.get(SwerveConstants.kCANbusName)); private final StatusSignal yawSignal = pigeon.getYaw(); private final StatusSignal yawRateSignal = pigeon.getAngularVelocityZWorld(); private final Queue odomTimestamps; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 6b71f497..68183f1c 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -27,8 +27,7 @@ import frc.robot.Constants.Cameras; import frc.robot.FieldConstants; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; -import java.util.LinkedList; -import java.util.List; +import java.util.ArrayList; import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { @@ -71,29 +70,30 @@ public Rotation2d getTargetX(int cameraIndex) { @Override public void periodic() { + // Update inputs + process inputs first (cheap, and keeps AK logs consistent) for (int i = 0; i < io.length; i++) { io[i].updateInputs(inputs[i]); - Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + Logger.processInputs("Vision/Camera" + i, inputs[i]); + disconnectedAlerts[i].set(!inputs[i].connected); } - // Initialize logging values - List allTagPoses = new LinkedList<>(); - List allRobotPoses = new LinkedList<>(); - List allRobotPosesAccepted = new LinkedList<>(); - List allRobotPosesRejected = new LinkedList<>(); + // Reusable scratch buffers (ArrayList avoids LinkedList churn) + // Tune these capacities if you know typical sizes + final ArrayList allTagPoses = new ArrayList<>(32); + final ArrayList allRobotPoses = new ArrayList<>(64); + final ArrayList allRobotPosesAccepted = new ArrayList<>(64); + final ArrayList allRobotPosesRejected = new ArrayList<>(64); // Loop over cameras for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { - // Update disconnected alert - disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); - // Initialize logging values - List tagPoses = new LinkedList<>(); - List robotPoses = new LinkedList<>(); - List robotPosesAccepted = new LinkedList<>(); - List robotPosesRejected = new LinkedList<>(); + // Per-camera scratch buffers + final ArrayList tagPoses = new ArrayList<>(16); + final ArrayList robotPoses = new ArrayList<>(32); + final ArrayList robotPosesAccepted = new ArrayList<>(32); + final ArrayList robotPosesRejected = new ArrayList<>(32); - // Add tag poses + // Add tag poses from ids for (int tagId : inputs[cameraIndex].tagIds) { var tagPose = FieldConstants.aprilTagLayout.getTagPose(tagId); if (tagPose.isPresent()) { @@ -151,19 +151,19 @@ public void periodic() { VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)); } - // Log camera datadata + // Log camera data Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses", - tagPoses.toArray(new Pose3d[0])); + "Vision/Camera" + cameraIndex + "/TagPoses", tagPoses.toArray(new Pose3d[0])); Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses", - robotPoses.toArray(new Pose3d[0])); + "Vision/Camera" + cameraIndex + "/RobotPoses", robotPoses.toArray(new Pose3d[0])); Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", + "Vision/Camera" + cameraIndex + "/RobotPosesAccepted", robotPosesAccepted.toArray(new Pose3d[0])); Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected", + "Vision/Camera" + cameraIndex + "/RobotPosesRejected", robotPosesRejected.toArray(new Pose3d[0])); + + // Summary aggregation allTagPoses.addAll(tagPoses); allRobotPoses.addAll(robotPoses); allRobotPosesAccepted.addAll(robotPosesAccepted); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 13f93b3b..f8d9cdd9 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -14,9 +14,8 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; +import java.util.ArrayList; import java.util.HashSet; -import java.util.LinkedList; -import java.util.List; import java.util.Set; import org.photonvision.PhotonCamera; @@ -40,10 +39,20 @@ public VisionIOPhotonVision(String name, Transform3d robotToCamera) { public void updateInputs(VisionIOInputs inputs) { inputs.connected = camera.isConnected(); - // Read new camera observations + // Cap the number of unread results processed per loop + final int kMaxUnread = 5; + + // Use HashSet/ArrayList to avoid LinkedList churn Set tagIds = new HashSet<>(); - List poseObservations = new LinkedList<>(); + ArrayList poseObservations = new ArrayList<>(kMaxUnread); + + int processed = 0; for (var result : camera.getAllUnreadResults()) { + // Hard cap + if (processed++ >= kMaxUnread) { + break; + } + // Update latest target observation if (result.hasTargets()) { inputs.latestTargetObservation = @@ -72,6 +81,10 @@ public void updateInputs(VisionIOInputs inputs) { // Add tag IDs tagIds.addAll(multitagResult.fiducialIDsUsed); + // Guard against divide-by-zero if targets is empty (defensive) + double avgTagDistance = + result.targets.isEmpty() ? 0.0 : (totalTagDistance / result.targets.size()); + // Add observation poseObservations.add( new PoseObservation( @@ -79,7 +92,7 @@ public void updateInputs(VisionIOInputs inputs) { robotPose, // 3D pose estimate multitagResult.estimatedPose.ambiguity, // Ambiguity multitagResult.fiducialIDsUsed.size(), // Tag count - totalTagDistance / result.targets.size(), // Average tag distance + avgTagDistance, // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type } else if (!result.targets.isEmpty()) { // Single tag result diff --git a/src/main/java/frc/robot/util/RBSIPowerMonitor.java b/src/main/java/frc/robot/util/RBSIPowerMonitor.java index 54972cf5..4d84b8f3 100644 --- a/src/main/java/frc/robot/util/RBSIPowerMonitor.java +++ b/src/main/java/frc/robot/util/RBSIPowerMonitor.java @@ -51,29 +51,33 @@ public class RBSIPowerMonitor extends VirtualSubsystem { RobotDevices.BR_ROTATION.getPowerPort() }; - // Class method definition, including inputs of optional subsystems + private final Alert totalCurrentAlert = + new Alert("Total current draw exceeds limit!", AlertType.WARNING); + + private final Alert[] portAlerts = new Alert[24]; // or pdh.getNumChannels() after construct + + // Constructor, including inputs of optional subsystems public RBSIPowerMonitor(LoggedTunableNumber batteryCapacityAh, RBSISubsystem... subsystems) { this.batteryCapacityAh = batteryCapacityAh; this.subsystems = subsystems; + + for (int i = 0; i < portAlerts.length; i++) { + portAlerts[i] = new Alert("Port " + i + " current exceeds limit!", AlertType.WARNING); + } } /** Periodic Method */ @Override - public void periodic() { + public void rbsiPeriodic() { // --- Read voltage & total current --- double voltage = m_pdm.getVoltage(); double totalCurrent = m_pdm.getTotalCurrent(); // --- Safety alerts --- - if (totalCurrent > PowerConstants.kTotalMaxCurrent) { - new Alert("Total current draw exceeds limit!", AlertType.WARNING).set(true); - } + totalCurrentAlert.set(totalCurrent > PowerConstants.kTotalMaxCurrent); for (int ch = 0; ch < m_pdm.getNumChannels(); ch++) { - double current = m_pdm.getCurrent(ch); - if (current > PowerConstants.kMotorPortMaxCurrent) { - new Alert("Port " + ch + " current exceeds limit!", AlertType.WARNING).set(true); - } + portAlerts[ch].set(m_pdm.getCurrent(ch) > PowerConstants.kMotorPortMaxCurrent); } // if (voltage < PowerConstants.kVoltageWarning) { diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java index 2eee8a68..803ae003 100644 --- a/src/main/java/frc/robot/util/RBSISubsystem.java +++ b/src/main/java/frc/robot/util/RBSISubsystem.java @@ -38,7 +38,8 @@ public abstract class RBSISubsystem extends SubsystemBase { public final void periodic() { long start = System.nanoTime(); rbsiPeriodic(); - Logger.recordOutput("LoggedRobot/" + name + "CodeMS", System.nanoTime() - start / 1e6); + long end = System.nanoTime(); + Logger.recordOutput("LoggedRobot/" + name + "CodeMS", (end - start) / 1e6); } /** Subclasses must implement this instead of periodic(). */ diff --git a/src/main/java/frc/robot/util/RobotDeviceId.java b/src/main/java/frc/robot/util/RobotDeviceId.java index 18005ffd..e51ccba1 100644 --- a/src/main/java/frc/robot/util/RobotDeviceId.java +++ b/src/main/java/frc/robot/util/RobotDeviceId.java @@ -50,6 +50,7 @@ public String getBus() { /** Get the CTRE CANBus object for a named device */ public CANBus getCANBus() { + return new CANBus(m_CANBus); } diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index 534aed08..8db7f562 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -11,12 +11,14 @@ import java.util.ArrayList; import java.util.List; +import org.littletonrobotics.junction.Logger; /** * Base class for virtual subsystems -- not robot hardware -- that should be treated as subsystems */ public abstract class VirtualSubsystem { private static List subsystems = new ArrayList<>(); + private final String name = getClass().getSimpleName(); // Load all defined virtual subsystems into a list public VirtualSubsystem() { @@ -30,6 +32,24 @@ public static void periodicAll() { } } - // Each virtual subsystem must implement its own periodic() method - public abstract void periodic(); + /** + * Guaranteed timing wrapper (cannot be bypassed by subclasses). + * + *

DO NOT OVERRIDE. + * + *

Subsystems must implement {@link #rbsiPeriodic()} instead. + * + *

If you see a compiler error here, remove your periodic() override and move your logic into + * rbsiPeriodic(). + */ + @Deprecated(forRemoval = false) + public final void periodic() { + long start = System.nanoTime(); + rbsiPeriodic(); + long end = System.nanoTime(); + Logger.recordOutput("LoggedRobot/" + name + "CodeMS", (end - start) / 1e6); + } + + /** Subclasses must implement this instead of periodic(). */ + protected abstract void rbsiPeriodic(); } From 02620e3e9ab6022a9a6bd360645b998090a3bd17 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 28 Jan 2026 14:32:08 -0700 Subject: [PATCH 11/14] More CAN cleanup by ading IMU Virtual subsystem --- src/main/java/frc/robot/RobotContainer.java | 26 ++++---- .../accelerometer/Accelerometer.java | 28 +++++---- .../frc/robot/subsystems/drive/Drive.java | 34 +++++------ .../java/frc/robot/subsystems/imu/Imu.java | 60 +++++++++++++++++++ .../java/frc/robot/subsystems/imu/ImuIO.java | 3 +- .../java/frc/robot/util/VirtualSubsystem.java | 3 +- 6 files changed, 107 insertions(+), 47 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/imu/Imu.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 531c4eeb..adef1292 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,6 +47,7 @@ import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; +import frc.robot.subsystems.imu.Imu; import frc.robot.subsystems.imu.ImuIO; import frc.robot.subsystems.imu.ImuIONavX; import frc.robot.subsystems.imu.ImuIOPigeon2; @@ -89,12 +90,13 @@ public class RobotContainer { // These are the "Active Subsystems" that the robot controls private final Drive m_drivebase; - private final ImuIO m_imu; private final Flywheel m_flywheel; // ... Add additional subsystems here (e.g., elevator, arm, etc.) // These are "Virtual Subsystems" that report information but have no motors + private final Imu m_imu; + @SuppressWarnings("unused") private final Accelerometer m_accel; @@ -136,17 +138,13 @@ public RobotContainer() { // YAGSL drivebase, get config from deploy directory // Get the IMU instance - switch (SwerveConstants.kImuType) { - case "pigeon2": - m_imu = new ImuIOPigeon2(); - break; - case "navx": - case "navx_spi": - m_imu = new ImuIONavX(); - break; - default: - throw new RuntimeException("Invalid IMU type"); - } + ImuIO imuIO = + switch (SwerveConstants.kImuType) { + case "pigeon2" -> new ImuIOPigeon2(); + case "navx", "navx_spi" -> new ImuIONavX(); + default -> throw new RuntimeException("Invalid IMU type"); + }; + m_imu = new Imu(imuIO); m_drivebase = new Drive(m_imu); m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); @@ -173,7 +171,7 @@ public RobotContainer() { case SIM: // Sim robot, instantiate physics sim IO implementations - m_imu = new ImuIOSim(); + m_imu = new Imu(new ImuIOSim() {}); m_drivebase = new Drive(m_imu); m_flywheel = new Flywheel(new FlywheelIOSim() {}); m_vision = @@ -207,7 +205,7 @@ public RobotContainer() { default: // Replayed robot, disable IO implementations - m_imu = new ImuIOSim(); + m_imu = new Imu(new ImuIOSim() {}); m_drivebase = new Drive(m_imu); m_flywheel = new Flywheel(new FlywheelIO() {}); m_vision = diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 4629b6a0..3981b7bb 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -18,7 +18,9 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.BuiltInAccelerometer; +import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; +import frc.robot.subsystems.imu.Imu; import frc.robot.subsystems.imu.ImuIO; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.Logger; @@ -34,36 +36,38 @@ public class Accelerometer extends VirtualSubsystem { private final BuiltInAccelerometer rioAccel = new BuiltInAccelerometer(); - private final ImuIO imuIO; - private final ImuIO.ImuIOInputs imuInputs = new ImuIO.ImuIOInputs(); + private final Imu imu; + + // RIO and IMU rotations with respect to the robot + private static final Rotation3d kRioRot = new Rotation3d(0, 0, kRioOrientation.getRadians()); + private static final Rotation3d kImuRot = new Rotation3d(0, 0, kIMUOrientation.getRadians()); private Translation3d prevRioAccel = Translation3d.kZero; - public Accelerometer(ImuIO imuIO) { - this.imuIO = imuIO; + public Accelerometer(Imu imu) { + this.imu = imu; } @Override public void rbsiPeriodic() { // --- Update IMU readings --- - imuIO.updateInputs(imuInputs); + final ImuIO.ImuIOInputs imuInputs = imu.getInputs(); // cached // --- Apply orientation corrections --- Translation3d rioAccVector = new Translation3d(rioAccel.getX(), rioAccel.getY(), rioAccel.getZ()) - .rotateBy(new Rotation3d(0., 0., kRioOrientation.getRadians())) + .rotateBy(kRioRot) .times(9.81); // convert to m/s^2 Translation3d imuAccVector = imuInputs .linearAccel - .rotateBy(new Rotation3d(0., 0., kIMUOrientation.getRadians())) + .rotateBy(kImuRot) .times(1.00); // already converted to m/s^2 in ImuIO implementation // --- Compute jerks --- Translation3d rioJerk = rioAccVector.minus(prevRioAccel).div(Constants.loopPeriodSecs); - Translation3d imuJerk = - imuInputs.jerk.rotateBy(new Rotation3d(0.0, 0.0, kIMUOrientation.getRadians())); + Translation3d imuJerk = imuInputs.jerk.rotateBy(kImuRot); // --- Log to AdvantageKit --- Logger.recordOutput("Accel/Rio/Accel_mps2", rioAccVector); @@ -73,9 +77,9 @@ public void rbsiPeriodic() { // --- Log IMU latency --- if (imuInputs.odometryYawTimestamps.length > 0) { - double latencySeconds = - System.currentTimeMillis() / 1000.0 - - imuInputs.odometryYawTimestamps[imuInputs.odometryYawTimestamps.length - 1]; + int last = imuInputs.odometryYawTimestamps.length - 1; + double now = Timer.getFPGATimestamp(); + double latencySeconds = now - imuInputs.odometryYawTimestamps[last]; Logger.recordOutput("IMU/LatencySec", latencySeconds); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 286a8636..5ac22d5d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -49,6 +49,7 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.RobotConstants; +import frc.robot.subsystems.imu.Imu; import frc.robot.subsystems.imu.ImuIO; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; @@ -62,15 +63,13 @@ public class Drive extends SubsystemBase { static final Lock odometryLock = new ReentrantLock(); - private final ImuIO imuIO; - private final ImuIO.ImuIOInputs imuInputs = new ImuIO.ImuIOInputs(); + private final Imu imu; private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine sysId; private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private Rotation2d rawGyroRotation = imuInputs.yawPosition; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { new SwerveModulePosition(), @@ -79,15 +78,15 @@ public class Drive extends SubsystemBase { new SwerveModulePosition() }; private SwerveDrivePoseEstimator m_PoseEstimator = - new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, Pose2d.kZero); + new SwerveDrivePoseEstimator(kinematics, Rotation2d.kZero, lastModulePositions, Pose2d.kZero); private ProfiledPIDController angleController; private DriveSimPhysics simPhysics; // Constructor - public Drive(ImuIO imuIO) { - this.imuIO = imuIO; + public Drive(Imu imu) { + this.imu = imu; // Define the Angle Controller angleController = @@ -117,7 +116,7 @@ public Drive(ImuIO imuIO) { for (int i = 0; i < 4; i++) { switch (modType) { case 0b00000000: // ALL-CTRE - if (kImuType == "navx" || kImuType == "navx_spi") { + if (kImuType.equals("navx") || kImuType.equals("navx_spi")) { modules[i] = new Module(new ModuleIOTalonFX(i), i); } else { throw new RuntimeException( @@ -223,6 +222,8 @@ public Drive(ImuIO imuIO) { public void periodic() { odometryLock.lock(); + final ImuIO.ImuIOInputs imuInputs = imu.getInputs(); + // Stop modules & log empty setpoint states if disabled if (DriverStation.isDisabled()) { for (var module : modules) { @@ -232,9 +233,6 @@ public void periodic() { } } - // Update the IMU inputs -- logging happens automatically - imuIO.updateInputs(imuInputs); - // Feed historical samples into odometry if REAL robot if (Constants.getMode() != Mode.SIM) { double[] sampleTimestamps = modules[0].getOdometryTimestamps(); @@ -275,7 +273,7 @@ public void periodic() { odometryLock.unlock(); // Update gyro/IMU alert - gyroDisconnectedAlert.set(!imuInputs.connected && Constants.getMode() != Mode.SIM); + gyroDisconnectedAlert.set(!imu.getInputs().connected && Constants.getMode() != Mode.SIM); } /** Simulation Periodic Method */ @@ -296,9 +294,9 @@ public void simulationPeriodic() { simPhysics.update(moduleStates, dt); // 4) Feed IMU from authoritative physics - imuIO.simulationSetYaw(simPhysics.getYaw()); - imuIO.simulationSetOmega(simPhysics.getOmegaRadPerSec()); - imuIO.setLinearAccel( + imu.simulationSetYaw(simPhysics.getYaw()); + imu.simulationSetOmega(simPhysics.getOmegaRadPerSec()); + imu.setLinearAccel( new Translation3d( simPhysics.getLinearAccel().getX(), simPhysics.getLinearAccel().getY(), 0.0)); @@ -461,7 +459,7 @@ public Rotation2d getHeading() { if (Constants.getMode() == Mode.SIM) { return simPhysics.getYaw(); } - return imuInputs.yawPosition; + return imu.getInputs().yawPosition; } /** Returns an array of module translations. */ @@ -541,12 +539,12 @@ public double getMaxAngularAccelRadPerSecPerSec() { /** Resets the current odometry pose. */ public void resetPose(Pose2d pose) { - m_PoseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + m_PoseEstimator.resetPosition(getHeading(), getModulePositions(), pose); } /** Zeros the gyro based on alliance color */ public void zeroHeadingForAlliance() { - imuIO.zeroYaw( + imu.zeroYaw( DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? Rotation2d.kZero : Rotation2d.k180deg); @@ -555,7 +553,7 @@ public void zeroHeadingForAlliance() { /** Zeros the heading */ public void zeroHeading() { - imuIO.zeroYaw(Rotation2d.kZero); + imu.zeroYaw(Rotation2d.kZero); resetHeadingController(); } diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java new file mode 100644 index 00000000..370b3556 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/imu/Imu.java @@ -0,0 +1,60 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.imu; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import frc.robot.util.VirtualSubsystem; +import org.littletonrobotics.junction.Logger; + +public class Imu extends VirtualSubsystem { + + private final ImuIO io; + private final ImuIOInputsAutoLogged inputs = new ImuIOInputsAutoLogged(); + + public Imu(ImuIO io) { + this.io = io; + } + + @Override + protected void rbsiPeriodic() { + io.updateInputs(inputs); + Logger.processInputs("IMU", inputs); // optional but useful + } + + public ImuIO.ImuIOInputs getInputs() { + return inputs; + } + + // Pass-throughs so Drive can still control the IMU + public void zeroYaw(Rotation2d yaw) { + io.zeroYaw(yaw); + } + + public void simulationSetYaw(Rotation2d yaw) { + io.simulationSetYaw(yaw); + } + + public void simulationSetOmega(double omegaRadPerSec) { + io.simulationSetOmega(omegaRadPerSec); + } + + public void setLinearAccel(Translation3d accelMps2) { + io.setLinearAccel(accelMps2); + } +} diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index 9273f0c9..d8c05f92 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -22,12 +22,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.util.RBSIIO; import org.littletonrobotics.junction.AutoLog; /** * Single IMU interface exposing all relevant state: orientation, rates, linear accel, and odometry. */ -public interface ImuIO { +public interface ImuIO extends RBSIIO { @AutoLog public static class ImuIOInputs { diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index 8db7f562..53f9d6e4 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -46,8 +46,7 @@ public static void periodicAll() { public final void periodic() { long start = System.nanoTime(); rbsiPeriodic(); - long end = System.nanoTime(); - Logger.recordOutput("LoggedRobot/" + name + "CodeMS", (end - start) / 1e6); + Logger.recordOutput("LoggedRobot/" + name + "CodeMS", (System.nanoTime() - start) / 1e6); } /** Subclasses must implement this instead of periodic(). */ From 0b23a7d75193562cefc8e47147f3eaea521db0af Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 28 Jan 2026 16:55:53 -0700 Subject: [PATCH 12/14] Prep for testing of loop cycle and CAN traffic Most of this commit is adding of testing messages and logging to investigate loop timing and to see what may be causing Stale CAN warnings. The extra logging and timing will likely be removed after the testing is complete and modifications to the loops made. --- src/main/java/frc/robot/Robot.java | 40 +++++++++++++++---- .../accelerometer/Accelerometer.java | 18 +++++---- .../frc/robot/subsystems/drive/Drive.java | 37 +++++++++++++---- .../java/frc/robot/subsystems/imu/Imu.java | 17 +++++++- .../java/frc/robot/util/RBSISubsystem.java | 2 +- .../java/frc/robot/util/VirtualSubsystem.java | 4 +- 6 files changed, 90 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d39bafa1..e9034e22 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -119,19 +119,45 @@ public Robot() { } } - /** This function is called periodically during all modes. */ + // /** This function is called periodically during all modes. */ + // @Override + // public void robotPeriodic() { + + // // Run all virtual subsystems each time through the loop + // VirtualSubsystem.periodicAll(); + + // // Runs the Scheduler. This is responsible for polling buttons, adding + // // newly-scheduled commands, running already-scheduled commands, removing + // // finished or interrupted commands, and running subsystem periodic() methods. + // // This must be called from the robot's periodic block in order for anything in + // // the Command-based framework to work. + // CommandScheduler.getInstance().run(); + // } + + /** TESTING VERSION OF ROBOTPERIODIC FOR OVERRUN SOURCES */ @Override public void robotPeriodic() { + final long t0 = System.nanoTime(); + + if (isReal()) { + Threads.setCurrentThreadPriority(true, 99); + } + final long t1 = System.nanoTime(); - // Run all virtual subsystems each time through the loop VirtualSubsystem.periodicAll(); + final long t2 = System.nanoTime(); - // Runs the Scheduler. This is responsible for polling buttons, adding - // newly-scheduled commands, running already-scheduled commands, removing - // finished or interrupted commands, and running subsystem periodic() methods. - // This must be called from the robot's periodic block in order for anything in - // the Command-based framework to work. CommandScheduler.getInstance().run(); + final long t3 = System.nanoTime(); + + Threads.setCurrentThreadPriority(false, 10); + final long t4 = System.nanoTime(); + + Logger.recordOutput("Loop/RobotPeriodic_ms", (t4 - t0) / 1e6); + Logger.recordOutput("Loop/ThreadBoost_ms", (t1 - t0) / 1e6); + Logger.recordOutput("Loop/Virtual_ms", (t2 - t1) / 1e6); + Logger.recordOutput("Loop/Scheduler_ms", (t3 - t2) / 1e6); + Logger.recordOutput("Loop/ThreadRestore_ms", (t4 - t3) / 1e6); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 3981b7bb..7752dd45 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; import frc.robot.subsystems.imu.Imu; -import frc.robot.subsystems.imu.ImuIO; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.Logger; @@ -50,8 +49,9 @@ public Accelerometer(Imu imu) { @Override public void rbsiPeriodic() { - // --- Update IMU readings --- - final ImuIO.ImuIOInputs imuInputs = imu.getInputs(); // cached + long t0 = System.nanoTime(); + // --- Updated IMU readings --- + final var imuInputs = imu.getInputs(); // --- Apply orientation corrections --- Translation3d rioAccVector = @@ -76,13 +76,15 @@ public void rbsiPeriodic() { Logger.recordOutput("Accel/IMU/Jerk_mps3", imuJerk); // --- Log IMU latency --- - if (imuInputs.odometryYawTimestamps.length > 0) { - int last = imuInputs.odometryYawTimestamps.length - 1; - double now = Timer.getFPGATimestamp(); - double latencySeconds = now - imuInputs.odometryYawTimestamps[last]; - Logger.recordOutput("IMU/LatencySec", latencySeconds); + final double[] ts = imuInputs.odometryYawTimestamps; + if (ts.length > 0) { + double latencySeconds = Timer.getFPGATimestamp() - ts[ts.length - 1]; + Logger.recordOutput("IMU/OdometryLatencySec", latencySeconds); } prevRioAccel = rioAccVector; + + long t1 = System.nanoTime(); + Logger.recordOutput("Loop/Accel/total_ms", (t1 - t0) / 1e6); } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5ac22d5d..a539b535 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -50,7 +50,6 @@ import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.RobotConstants; import frc.robot.subsystems.imu.Imu; -import frc.robot.subsystems.imu.ImuIO; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIParsing; @@ -220,9 +219,12 @@ public Drive(Imu imu) { /** Periodic function that is called each robot cycle by the command scheduler */ @Override public void periodic() { + final long t0 = System.nanoTime(); odometryLock.lock(); + final long t1 = System.nanoTime(); - final ImuIO.ImuIOInputs imuInputs = imu.getInputs(); + final var imuInputs = imu.getInputs(); + final long t2 = System.nanoTime(); // Stop modules & log empty setpoint states if disabled if (DriverStation.isDisabled()) { @@ -232,17 +234,18 @@ public void periodic() { Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } } + final long t3 = System.nanoTime(); // Feed historical samples into odometry if REAL robot if (Constants.getMode() != Mode.SIM) { double[] sampleTimestamps = modules[0].getOdometryTimestamps(); int sampleCount = sampleTimestamps.length; - for (int i = 0; i < sampleCount; i++) { - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int i = 0; i < sampleCount; i++) { for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; moduleDeltas[moduleIndex] = @@ -264,16 +267,34 @@ public void periodic() { } Logger.recordOutput("Drive/Pose", m_PoseEstimator.getEstimatedPosition()); } + final long t4 = System.nanoTime(); // Module periodic updates for (var module : modules) { module.periodic(); } + final long t5 = System.nanoTime(); odometryLock.unlock(); + final long t6 = System.nanoTime(); + + Logger.recordOutput("Loop/Drive/total_ms", (t6 - t0) / 1e6); + Logger.recordOutput("Loop/Drive/lockWait_ms", (t1 - t0) / 1e6); + Logger.recordOutput("Loop/Drive/getImuInputs_ms", (t2 - t1) / 1e6); + Logger.recordOutput("Loop/Drive/disabled_ms", (t3 - t2) / 1e6); + Logger.recordOutput("Loop/Drive/odometry_ms", (t4 - t3) / 1e6); + Logger.recordOutput("Loop/Drive/modules_ms", (t5 - t4) / 1e6); + Logger.recordOutput("Loop/Drive/unlock_ms", (t6 - t5) / 1e6); + + double driveMs = (t6 - t0) / 1e6; + Logger.recordOutput("Loop/Drive/total_ms", driveMs); + + if (driveMs > 20.0) { + Logger.recordOutput("LoopSpike/Drive/odometry_ms", (t4 - t3) / 1e6); + Logger.recordOutput("LoopSpike/Drive/modules_ms", (t5 - t4) / 1e6); + } - // Update gyro/IMU alert - gyroDisconnectedAlert.set(!imu.getInputs().connected && Constants.getMode() != Mode.SIM); + gyroDisconnectedAlert.set(!imuInputs.connected && Constants.getMode() != Mode.SIM); } /** Simulation Periodic Method */ diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java index 370b3556..c69a06bd 100644 --- a/src/main/java/frc/robot/subsystems/imu/Imu.java +++ b/src/main/java/frc/robot/subsystems/imu/Imu.java @@ -33,11 +33,24 @@ public Imu(ImuIO io) { @Override protected void rbsiPeriodic() { + final long t0 = System.nanoTime(); io.updateInputs(inputs); - Logger.processInputs("IMU", inputs); // optional but useful + final long t1 = System.nanoTime(); + Logger.processInputs("IMU", inputs); + final long t2 = System.nanoTime(); + + Logger.recordOutput("Loop/IMU/update_ms", (t1 - t0) / 1e6); + Logger.recordOutput("Loop/IMU/log_ms", (t2 - t1) / 1e6); + Logger.recordOutput("Loop/IMU/total_ms", (t2 - t0) / 1e6); + + double totalMs = (t2 - t0) / 1e6; + Logger.recordOutput("Loop/IMU/total_ms", totalMs); + if (totalMs > 2.0) { + Logger.recordOutput("LoopSpike/IMU/update_ms", (t1 - t0) / 1e6); + } } - public ImuIO.ImuIOInputs getInputs() { + public ImuIOInputsAutoLogged getInputs() { return inputs; } diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java index 803ae003..3aa3b8b9 100644 --- a/src/main/java/frc/robot/util/RBSISubsystem.java +++ b/src/main/java/frc/robot/util/RBSISubsystem.java @@ -39,7 +39,7 @@ public final void periodic() { long start = System.nanoTime(); rbsiPeriodic(); long end = System.nanoTime(); - Logger.recordOutput("LoggedRobot/" + name + "CodeMS", (end - start) / 1e6); + Logger.recordOutput("Loop/Mech/" + name + "_ms", (end - start) / 1e6); } /** Subclasses must implement this instead of periodic(). */ diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index 53f9d6e4..2996aa33 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -17,7 +17,7 @@ * Base class for virtual subsystems -- not robot hardware -- that should be treated as subsystems */ public abstract class VirtualSubsystem { - private static List subsystems = new ArrayList<>(); + private static final List subsystems = new ArrayList<>(); private final String name = getClass().getSimpleName(); // Load all defined virtual subsystems into a list @@ -46,7 +46,7 @@ public static void periodicAll() { public final void periodic() { long start = System.nanoTime(); rbsiPeriodic(); - Logger.recordOutput("LoggedRobot/" + name + "CodeMS", (System.nanoTime() - start) / 1e6); + Logger.recordOutput("Loop/Virtual/" + name + "_ms", (System.nanoTime() - start) / 1e6); } /** Subclasses must implement this instead of periodic(). */ From 3e77c7ae0363977b73135335e8eea99b503408e9 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Jan 2026 10:57:39 -0700 Subject: [PATCH 13/14] Now measure CAN health; move CANBus instantiation to `util/` We can now measure and log CAN health (new utility), and have moved the registration and creation of the CANBus objects to a utility out of the `Constants.java` file. Conformal changes throughout. --- src/main/java/frc/robot/Constants.java | 40 ------- src/main/java/frc/robot/RobotContainer.java | 21 +++- .../frc/robot/subsystems/drive/Drive.java | 16 +-- .../subsystems/drive/ModuleIOBlended.java | 4 +- .../drive/ModuleIOSparkCANcoder.java | 4 +- .../subsystems/drive/ModuleIOTalonFX.java | 56 +++++++--- .../subsystems/drive/ModuleIOTalonFXS.java | 4 +- .../drive/PhoenixOdometryThread.java | 2 +- .../subsystems/drive/SparkOdometryThread.java | 2 +- .../robot/subsystems/imu/ImuIOPigeon2.java | 5 +- .../frc/robot/util/RBSICANBusRegistry.java | 101 ++++++++++++++++++ .../java/frc/robot/util/RBSICANHealth.java | 36 +++++++ .../java/frc/robot/util/RBSIPowerMonitor.java | 23 ++-- 13 files changed, 227 insertions(+), 87 deletions(-) create mode 100644 src/main/java/frc/robot/util/RBSICANBusRegistry.java create mode 100644 src/main/java/frc/robot/util/RBSICANHealth.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5d4111ca..cfb7214b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,7 +19,6 @@ import static edu.wpi.first.units.Units.*; -import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; @@ -46,9 +45,6 @@ import frc.robot.util.RBSIEnum.SwerveType; import frc.robot.util.RBSIEnum.VisionType; import frc.robot.util.RobotDeviceId; -import java.util.Collections; -import java.util.HashMap; -import java.util.Map; import org.photonvision.simulation.SimCameraProperties; import swervelib.math.Matter; @@ -168,44 +164,8 @@ public static final class PowerConstants { /************************************************************************* */ /** List of Robot CAN Busses ********************************************* */ public static final class CANBuses { - - // ---- Bus names (single source of truth) ---- public static final String DRIVE = "DriveTrain"; public static final String RIO = ""; - // In 2027 and later, you'll be able to have even more CAN Busses! - - // ---- Singleton instances (exactly one per bus) ---- - public static final CANBus DRIVE_BUS = new CANBus(DRIVE); - public static final CANBus RIO_BUS = new CANBus(RIO); - - // ---- Lookup table: name -> CANBus singleton ---- - private static final Map BY_NAME; - - static { - Map m = new HashMap<>(); - m.put(DRIVE, DRIVE_BUS); - m.put(RIO, RIO_BUS); - BY_NAME = Collections.unmodifiableMap(m); - } - - /** - * Get the singleton CANBus for a given bus name. - * - *

Usage: CANBus bus = Constants.CANBuses.get(Constants.CANBuses.DRIVE); - */ - public static CANBus get(String name) { - CANBus bus = BY_NAME.get(name); - if (bus == null) { - throw new IllegalArgumentException( - "Unknown CAN bus name '" + name + "'. Known buses: " + BY_NAME.keySet()); - } - return bus; - } - - /** Expose known bus names for debugging. */ - public static Map all() { - return BY_NAME; - } } /************************************************************************* */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d8e516db..10bcd058 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,6 +36,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.CANBuses; import frc.robot.Constants.OperatorConstants; import frc.robot.Constants.SimCameras; import frc.robot.FieldConstants.AprilTagLayoutType; @@ -63,6 +64,8 @@ import frc.robot.util.GetJoystickValue; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.OverrideSwitches; +import frc.robot.util.RBSICANBusRegistry; +import frc.robot.util.RBSICANHealth; import frc.robot.util.RBSIEnum.AutoType; import frc.robot.util.RBSIEnum.DriveStyle; import frc.robot.util.RBSIEnum.Mode; @@ -107,6 +110,9 @@ public class RobotContainer { @SuppressWarnings("unused") private final Vision m_vision; + @SuppressWarnings("unused") + private RBSICANHealth m_can0, m_can1; + /** Dashboard inputs ***************************************************** */ // AutoChoosers for both supported path planning types private final LoggedDashboardChooser autoChooserPathPlanner; @@ -136,8 +142,11 @@ public RobotContainer() { switch (Constants.getMode()) { case REAL: // Real robot, instantiate hardware IO implementations - // YAGSL drivebase, get config from deploy directory + // Register the CANBus + RBSICANBusRegistry.initReal(CANBuses.RIO, CANBuses.DRIVE); + + // YAGSL drivebase, get config from deploy directory // Get the IMU instance ImuIO imuIO = switch (SwerveConstants.kImuType) { @@ -168,10 +177,14 @@ public RobotContainer() { }; m_accel = new Accelerometer(m_imu); sweep = null; + m_can0 = new RBSICANHealth(CANBuses.RIO); + m_can1 = new RBSICANHealth(CANBuses.DRIVE); + break; case SIM: // Sim robot, instantiate physics sim IO implementations + RBSICANBusRegistry.initSim(CANBuses.RIO, CANBuses.DRIVE); m_imu = new Imu(new ImuIOSim() {}); m_drivebase = new Drive(m_imu); m_flywheel = new Flywheel(new FlywheelIOSim() {}); @@ -201,11 +214,13 @@ public RobotContainer() { visionSim.addCamera(cam2, Transform3d.kZero); // 5) Create the sweep evaluator sweep = new CameraSweepEvaluator(visionSim, cam1, cam2); - + m_can0 = new RBSICANHealth(CANBuses.RIO); + m_can1 = new RBSICANHealth(CANBuses.DRIVE); break; default: // Replayed robot, disable IO implementations + RBSICANBusRegistry.initSim(CANBuses.RIO, CANBuses.DRIVE); m_imu = new Imu(new ImuIOSim() {}); m_drivebase = new Drive(m_imu); m_flywheel = new Flywheel(new FlywheelIO() {}); @@ -213,6 +228,8 @@ public RobotContainer() { new Vision(m_drivebase::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); m_accel = new Accelerometer(m_imu); sweep = null; + m_can0 = new RBSICANHealth(CANBuses.RIO); + m_can1 = new RBSICANHealth(CANBuses.DRIVE); break; } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index a539b535..16c06ef3 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -236,6 +236,12 @@ public void periodic() { } final long t3 = System.nanoTime(); + // Module periodic updates, which drains queues this cycle + for (var module : modules) { + module.periodic(); + } + final long t4 = System.nanoTime(); + // Feed historical samples into odometry if REAL robot if (Constants.getMode() != Mode.SIM) { double[] sampleTimestamps = modules[0].getOdometryTimestamps(); @@ -267,12 +273,6 @@ public void periodic() { } Logger.recordOutput("Drive/Pose", m_PoseEstimator.getEstimatedPosition()); } - final long t4 = System.nanoTime(); - - // Module periodic updates - for (var module : modules) { - module.periodic(); - } final long t5 = System.nanoTime(); odometryLock.unlock(); @@ -282,8 +282,8 @@ public void periodic() { Logger.recordOutput("Loop/Drive/lockWait_ms", (t1 - t0) / 1e6); Logger.recordOutput("Loop/Drive/getImuInputs_ms", (t2 - t1) / 1e6); Logger.recordOutput("Loop/Drive/disabled_ms", (t3 - t2) / 1e6); - Logger.recordOutput("Loop/Drive/odometry_ms", (t4 - t3) / 1e6); - Logger.recordOutput("Loop/Drive/modules_ms", (t5 - t4) / 1e6); + Logger.recordOutput("Loop/Drive/modules_ms", (t4 - t3) / 1e6); + Logger.recordOutput("Loop/Drive/odometry_ms", (t5 - t4) / 1e6); Logger.recordOutput("Loop/Drive/unlock_ms", (t6 - t5) / 1e6); double driveMs = (t6 - t0) / 1e6; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 6319902c..bac7c403 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -52,9 +52,9 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; -import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.PhoenixUtil; +import frc.robot.util.RBSICANBusRegistry; import frc.robot.util.SparkUtil; import java.util.Queue; import org.littletonrobotics.junction.Logger; @@ -183,7 +183,7 @@ public ModuleIOBlended(int module) { default -> throw new IllegalArgumentException("Invalid module index"); }; - CANBus canBus = CANBuses.get(SwerveConstants.kCANbusName); + CANBus canBus = RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName); driveTalon = new TalonFX(constants.DriveMotorId, canBus); turnSpark = new SparkMax(constants.SteerMotorId, MotorType.kBrushless); cancoder = new CANcoder(constants.EncoderId, canBus); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index 73270a7f..fc76b593 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -35,8 +35,8 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; -import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; +import frc.robot.util.RBSICANBusRegistry; import frc.robot.util.SparkUtil; import java.util.Queue; import java.util.function.DoubleSupplier; @@ -141,7 +141,7 @@ public ModuleIOSparkCANcoder(int module) { case 3 -> SwerveConstants.kBREncoderId; default -> 0; }, - CANBuses.get(SwerveConstants.kCANbusName)); + RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName)); driveController = driveSpark.getClosedLoopController(); turnController = turnSpark.getClosedLoopController(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index b616b0be..45818586 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -44,10 +44,10 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; -import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.generated.TunerConstants; import frc.robot.util.PhoenixUtil; +import frc.robot.util.RBSICANBusRegistry; import java.util.Queue; import org.littletonrobotics.junction.Logger; @@ -62,6 +62,9 @@ public class ModuleIOTalonFX implements ModuleIO { TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants; + // This module number (for logging) + private final int module; + // Hardware objects private final TalonFX driveTalon; private final TalonFX turnTalon; @@ -94,6 +97,7 @@ public class ModuleIOTalonFX implements ModuleIO { // Inputs from drive motor private final StatusSignal drivePosition; + private final StatusSignal drivePositionOdom; private final Queue drivePositionQueue; private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; @@ -102,6 +106,7 @@ public class ModuleIOTalonFX implements ModuleIO { // Inputs from turn motor private final StatusSignal turnAbsolutePosition; private final StatusSignal turnPosition; + private final StatusSignal turnPositionOdom; private final Queue turnPositionQueue; private final StatusSignal turnVelocity; private final StatusSignal turnAppliedVolts; @@ -127,6 +132,9 @@ public class ModuleIOTalonFX implements ModuleIO { * TalonFX I/O */ public ModuleIOTalonFX(int module) { + // Record the module number for logging purposes + this.module = module; + constants = switch (module) { case 0 -> TunerConstants.FrontLeft; @@ -136,7 +144,7 @@ public ModuleIOTalonFX(int module) { default -> throw new IllegalArgumentException("Invalid module index"); }; - CANBus canBus = CANBuses.get(SwerveConstants.kCANbusName); + CANBus canBus = RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName); driveTalon = new TalonFX(constants.DriveMotorId, canBus); turnTalon = new TalonFX(constants.SteerMotorId, canBus); cancoder = new CANcoder(constants.EncoderId, canBus); @@ -217,23 +225,26 @@ public ModuleIOTalonFX(int module) { // Create drive status signals drivePosition = driveTalon.getPosition(); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon.getPosition()); + drivePositionOdom = drivePosition.clone(); // NEW + drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePositionOdom); + driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getStatorCurrent(); // Create turn status signals - turnAbsolutePosition = cancoder.getAbsolutePosition(); turnPosition = turnTalon.getPosition(); - turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnTalon.getPosition()); + turnPositionOdom = turnPosition.clone(); // NEW + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPositionOdom); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); turnCurrent = turnTalon.getStatorCurrent(); - // Configure periodic frames + // Configure periodic frames (IMPORTANT: apply odometry rate to the *odom clones*) BaseStatusSignal.setUpdateFrequencyForAll( - SwerveConstants.kOdometryFrequency, drivePosition, turnPosition); + SwerveConstants.kOdometryFrequency, drivePositionOdom, turnPositionOdom); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, driveVelocity, @@ -243,17 +254,25 @@ public ModuleIOTalonFX(int module) { turnVelocity, turnAppliedVolts, turnCurrent); + ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); } @Override public void updateInputs(ModuleIOInputs inputs) { - // Refresh all signals - var driveStatus = - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - var turnStatus = - BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); - var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + + // Refresh most signals + long a0 = System.nanoTime(); + var driveStatus = BaseStatusSignal.refreshAll(driveVelocity, driveAppliedVolts, driveCurrent); + long a1 = System.nanoTime(); + var turnStatus = BaseStatusSignal.refreshAll(turnVelocity, turnAppliedVolts, turnCurrent); + long a2 = System.nanoTime(); + var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + long a3 = System.nanoTime(); + + Logger.recordOutput("LoopSpike/Module" + module + "/refresh_drive_ms", (a1 - a0) / 1e6); + Logger.recordOutput("LoopSpike/Module" + module + "/refresh_turn_ms", (a2 - a1) / 1e6); + Logger.recordOutput("LoopSpike/Module" + module + "/refresh_enc_ms", (a3 - a2) / 1e6); // Update drive inputs inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); @@ -264,7 +283,7 @@ public void updateInputs(ModuleIOInputs inputs) { // Update turn inputs inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); - inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(encStatus.isOK()); inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); @@ -282,6 +301,13 @@ public void updateInputs(ModuleIOInputs inputs) { turnPositionQueue.stream() .map((Double value) -> Rotation2d.fromRotations(value)) .toArray(Rotation2d[]::new); + + // Log queue size right before we clear + Logger.recordOutput("LoopSpike/Module" + module + "/queue_ts_len", timestampQueue.size()); + Logger.recordOutput( + "LoopSpike/Module" + module + "/queue_drive_len", drivePositionQueue.size()); + Logger.recordOutput("LoopSpike/Module" + module + "/queue_turn_len", turnPositionQueue.size()); + timestampQueue.clear(); drivePositionQueue.clear(); turnPositionQueue.clear(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index 34bdf72c..a0b6caae 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -45,9 +45,9 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; -import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.PhoenixUtil; +import frc.robot.util.RBSICANBusRegistry; import java.util.Queue; import org.littletonrobotics.junction.Logger; @@ -125,7 +125,7 @@ public class ModuleIOTalonFXS implements ModuleIO { public ModuleIOTalonFXS( SwerveModuleConstants constants) { - CANBus canBus = CANBuses.get(SwerveConstants.kCANbusName); + CANBus canBus = RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName); driveTalon = new TalonFXS(constants.DriveMotorId, canBus); turnTalon = new TalonFXS(constants.SteerMotorId, canBus); candi = new CANdi(constants.EncoderId, canBus); diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 188f987f..09bc9fe2 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -96,7 +96,7 @@ public Queue registerSignal(DoubleSupplier signal) { /** Returns a new queue that returns timestamp values for each sample. */ public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>(20); + Queue queue = new ArrayBlockingQueue<>(128); // was 20 Drive.odometryLock.lock(); try { timestampQueues.add(queue); diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java index c97fc503..ec377d4e 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -82,7 +82,7 @@ public Queue registerSignal(DoubleSupplier signal) { /** Returns a new queue that returns timestamp values for each sample. */ public Queue makeTimestampQueue() { - Queue queue = new ArrayBlockingQueue<>(20); + Queue queue = new ArrayBlockingQueue<>(128); // was 20 Drive.odometryLock.lock(); try { timestampQueues.add(queue); diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java index 664dafb9..7aef3faa 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java @@ -28,16 +28,17 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import frc.robot.Constants.CANBuses; import frc.robot.subsystems.drive.PhoenixOdometryThread; import frc.robot.subsystems.drive.SwerveConstants; +import frc.robot.util.RBSICANBusRegistry; import java.util.Queue; /** IMU IO for CTRE Pigeon2 */ public class ImuIOPigeon2 implements ImuIO { private final Pigeon2 pigeon = - new Pigeon2(SwerveConstants.kPigeonId, CANBuses.get(SwerveConstants.kCANbusName)); + new Pigeon2( + SwerveConstants.kPigeonId, RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName)); private final StatusSignal yawSignal = pigeon.getYaw(); private final StatusSignal yawRateSignal = pigeon.getAngularVelocityZWorld(); private final Queue odomTimestamps; diff --git a/src/main/java/frc/robot/util/RBSICANBusRegistry.java b/src/main/java/frc/robot/util/RBSICANBusRegistry.java new file mode 100644 index 00000000..f247cde0 --- /dev/null +++ b/src/main/java/frc/robot/util/RBSICANBusRegistry.java @@ -0,0 +1,101 @@ +package frc.robot.util; + +import com.ctre.phoenix6.CANBus; +import java.util.Map; +import java.util.concurrent.ConcurrentHashMap; + +/** Centralized CAN bus singleton registry + SIM/REAL indirection. */ +public final class RBSICANBusRegistry { + private static final Map realBuses = new ConcurrentHashMap<>(); + private static final Map likeBuses = new ConcurrentHashMap<>(); + private static volatile boolean initialized = false; + private static volatile boolean sim = false; + + public static void initReal(String... busNames) { + sim = false; + for (String name : busNames) { + CANBus bus = realBuses.computeIfAbsent(name, CANBus::new); + likeBuses.computeIfAbsent(name, n -> new RealCANBusAdapter(bus)); + } + initialized = true; + } + + public static void initSim(String... busNames) { + sim = true; + for (String name : busNames) { + likeBuses.computeIfAbsent(name, SimCANBusStub::new); + } + initialized = true; + } + + /** For Phoenix device constructors (REAL/REPLAY only). */ + public static CANBus getBus(String name) { + checkInit(); + if (sim) { + throw new IllegalStateException("No real CANBus in SIM. Use getLike() or skip CTRE devices."); + } + CANBus bus = realBuses.get(name); + if (bus == null) throwUnknown(name, realBuses.keySet()); + return bus; + } + + /** For health logging (REAL or SIM). */ + public static CANBusLike getLike(String name) { + checkInit(); + CANBusLike bus = likeBuses.get(name); + if (bus == null) throwUnknown(name, likeBuses.keySet()); + return bus; + } + + private static void checkInit() { + if (!initialized) throw new IllegalStateException("RBSICANBusRegistry not initialized."); + } + + private static void throwUnknown(String name, java.util.Set known) { + throw new IllegalArgumentException("Unknown CAN bus '" + name + "'. Known: " + known); + } + + // ---- nested types ---- + + public interface CANBusLike { + String getName(); + + CANBus.CANBusStatus getStatus(); + } + + static final class RealCANBusAdapter implements CANBusLike { + private final CANBus bus; + + RealCANBusAdapter(CANBus bus) { + this.bus = bus; + } + + @Override + public String getName() { + return bus.getName(); + } + + @Override + public CANBus.CANBusStatus getStatus() { + return bus.getStatus(); + } + } + + static final class SimCANBusStub implements CANBusLike { + private final String name; + + SimCANBusStub(String name) { + this.name = name; + } + + @Override + public String getName() { + return name; + } + + @Override + public CANBus.CANBusStatus getStatus() { + return new CANBus.CANBusStatus(); + } + } +} diff --git a/src/main/java/frc/robot/util/RBSICANHealth.java b/src/main/java/frc/robot/util/RBSICANHealth.java new file mode 100644 index 00000000..901d6632 --- /dev/null +++ b/src/main/java/frc/robot/util/RBSICANHealth.java @@ -0,0 +1,36 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.util; + +import org.littletonrobotics.junction.Logger; + +public class RBSICANHealth extends VirtualSubsystem { + private long loops = 0; + private final RBSICANBusRegistry.CANBusLike bus; + + public RBSICANHealth(String busName) { + bus = RBSICANBusRegistry.getLike(busName); + } + + @Override + protected void rbsiPeriodic() { + if ((loops++ % 5) != 0) return; + var status = bus.getStatus(); + Logger.recordOutput("CAN/" + bus.getName() + "/Utilization", status.BusUtilization); + Logger.recordOutput("CAN/" + bus.getName() + "/TxFullCount", status.TxFullCount); + Logger.recordOutput("CAN/" + bus.getName() + "/REC", status.REC); + Logger.recordOutput("CAN/" + bus.getName() + "/TEC", status.TEC); + Logger.recordOutput("CAN/" + bus.getName() + "/BusOffCount", status.BusOffCount); + } +} diff --git a/src/main/java/frc/robot/util/RBSIPowerMonitor.java b/src/main/java/frc/robot/util/RBSIPowerMonitor.java index d01e87d5..ba6daadf 100644 --- a/src/main/java/frc/robot/util/RBSIPowerMonitor.java +++ b/src/main/java/frc/robot/util/RBSIPowerMonitor.java @@ -54,8 +54,12 @@ public class RBSIPowerMonitor extends VirtualSubsystem { private final Alert totalCurrentAlert = new Alert("Total current draw exceeds limit!", AlertType.WARNING); - private final Alert[] portAlerts = new Alert[24]; // or pdh.getNumChannels() after construct + private final Alert lowVoltageAlert = new Alert("Low battery voltage!", AlertType.WARNING); + private final Alert criticalVoltageAlert = + new Alert("Critical battery voltage!", AlertType.ERROR); + + private long loops = 0; // Constructor, including inputs of optional subsystems public RBSIPowerMonitor(LoggedTunableNumber batteryCapacityAh, RBSISubsystem... subsystems) { @@ -70,25 +74,20 @@ public RBSIPowerMonitor(LoggedTunableNumber batteryCapacityAh, RBSISubsystem... /** Periodic Method */ @Override public void rbsiPeriodic() { + // Limit polling to every 5 loops + if ((loops++ % 5) != 0) return; // 50Hz loop -> run at 10Hz + // --- Read voltage & total current --- double voltage = conduit.getPDPVoltage(); double totalCurrent = conduit.getPDPTotalCurrent(); // --- 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++) { - double current = conduit.getPDPChannelCurrent(ch); - if (current > PowerConstants.kMotorPortMaxCurrent) { - new Alert("Port " + ch + " current exceeds limit!", AlertType.WARNING).set(true); - } - } - - if (voltage < PowerConstants.kVoltageWarning) { - new Alert("Low battery voltage!", AlertType.WARNING).set(true); - } - if (voltage < PowerConstants.kVoltageCritical) { - new Alert("Critical battery voltage!", AlertType.ERROR).set(true); + portAlerts[ch].set(conduit.getPDPChannelCurrent(ch) > PowerConstants.kMotorPortMaxCurrent); } // --- Battery estimation --- From a77e26e587a7c9a7e851b9eba970ee2aaed82b9c Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Fri, 30 Jan 2026 18:35:02 -0700 Subject: [PATCH 14/14] Bring loop times down --- .../accelerometer/Accelerometer.java | 142 +++++++++----- .../subsystems/accelerometer/RioAccelIO.java | 26 +++ .../accelerometer/RioAccelIORoboRIO.java | 53 ++++++ .../frc/robot/subsystems/drive/Drive.java | 119 ++++++------ .../subsystems/drive/ModuleIOBlended.java | 12 +- .../subsystems/drive/ModuleIOTalonFX.java | 32 ++-- .../subsystems/drive/ModuleIOTalonFXS.java | 19 +- .../drive/PhoenixOdometryThread.java | 17 +- .../subsystems/drive/SparkOdometryThread.java | 4 +- .../java/frc/robot/subsystems/imu/Imu.java | 80 +++++--- .../java/frc/robot/subsystems/imu/ImuIO.java | 58 +++--- .../frc/robot/subsystems/imu/ImuIONavX.java | 162 ++++++++++++---- .../robot/subsystems/imu/ImuIOPigeon2.java | 179 ++++++++++++------ .../frc/robot/subsystems/imu/ImuIOSim.java | 118 +++++++----- .../frc/robot/subsystems/vision/Vision.java | 9 +- .../frc/robot/util/RBSICANBusRegistry.java | 13 ++ .../java/frc/robot/util/RBSICANHealth.java | 4 +- 17 files changed, 698 insertions(+), 349 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java create mode 100644 src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 7752dd45..3bfc5c1f 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -13,78 +13,124 @@ package frc.robot.subsystems.accelerometer; -import static frc.robot.Constants.RobotConstants.*; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.BuiltInAccelerometer; import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; +import frc.robot.Constants.RobotConstants; import frc.robot.subsystems.imu.Imu; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.Logger; /** - * Accelerometer subsystem (built upon a virtual subsystem) + * Accelerometer subsystem (VirtualSubsystem) * *

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

Primitive-only hot path: no WPILib geometry objects or Units objects. */ public class Accelerometer extends VirtualSubsystem { + private static final double G_TO_MPS2 = 9.80665; - private final BuiltInAccelerometer rioAccel = new BuiltInAccelerometer(); + private final RioAccelIO rio; + private final RioAccelIO.Inputs rioInputs = new RioAccelIO.Inputs(); private final Imu imu; - // RIO and IMU rotations with respect to the robot - private static final Rotation3d kRioRot = new Rotation3d(0, 0, kRioOrientation.getRadians()); - private static final Rotation3d kImuRot = new Rotation3d(0, 0, kIMUOrientation.getRadians()); + // Precompute yaw-only rotation terms + private static final double rioCos = Math.cos(RobotConstants.kRioOrientation.getRadians()); + private static final double rioSin = Math.sin(RobotConstants.kRioOrientation.getRadians()); + private static final double imuCos = Math.cos(RobotConstants.kIMUOrientation.getRadians()); + private static final double imuSin = Math.sin(RobotConstants.kIMUOrientation.getRadians()); + + // Previous Rio accel (m/s^2) + private double prevRioAx = 0.0, prevRioAy = 0.0, prevRioAz = 0.0; + + // Reusable arrays for logging + private final double[] rioAccelArr = new double[3]; + private final double[] rioJerkArr = new double[3]; + private final double[] imuAccelArr = new double[3]; + private final double[] imuJerkArr = new double[3]; - private Translation3d prevRioAccel = Translation3d.kZero; + // Log decimation + private int loopCount = 0; + private static final int LOG_EVERY_N = 5; // 10Hz for heavier logs + + // Profiling decimation + private int profileCount = 0; + private static final int PROFILE_EVERY_N = 50; // 1Hz profiling public Accelerometer(Imu imu) { this.imu = imu; + this.rio = new RioAccelIORoboRIO(200.0); // 200 Hz is a good start } @Override public void rbsiPeriodic() { - long t0 = System.nanoTime(); - // --- Updated IMU readings --- - final var imuInputs = imu.getInputs(); - - // --- Apply orientation corrections --- - Translation3d rioAccVector = - new Translation3d(rioAccel.getX(), rioAccel.getY(), rioAccel.getZ()) - .rotateBy(kRioRot) - .times(9.81); // convert to m/s^2 - - Translation3d imuAccVector = - imuInputs - .linearAccel - .rotateBy(kImuRot) - .times(1.00); // already converted to m/s^2 in ImuIO implementation - - // --- Compute jerks --- - Translation3d rioJerk = rioAccVector.minus(prevRioAccel).div(Constants.loopPeriodSecs); - Translation3d imuJerk = imuInputs.jerk.rotateBy(kImuRot); - - // --- Log to AdvantageKit --- - Logger.recordOutput("Accel/Rio/Accel_mps2", rioAccVector); - Logger.recordOutput("Accel/Rio/Jerk_mps3", rioJerk); - Logger.recordOutput("Accel/IMU/Accel_mps2", imuAccVector); - Logger.recordOutput("Accel/IMU/Jerk_mps3", imuJerk); - - // --- Log IMU latency --- - final double[] ts = imuInputs.odometryYawTimestamps; - if (ts.length > 0) { - double latencySeconds = Timer.getFPGATimestamp() - ts[ts.length - 1]; - Logger.recordOutput("IMU/OdometryLatencySec", latencySeconds); + final boolean doProfile = (++profileCount >= PROFILE_EVERY_N); + if (doProfile) profileCount = 0; + + // Fetch the values from the IMU and the RIO + final var imuInputs = imu.getInputs(); // should be primitive ImuIOInputs + rio.updateInputs(rioInputs); + + // Compute RIO accelerations and jerks + final double rawX = rioInputs.xG; + final double rawY = rioInputs.yG; + final double rawZ = rioInputs.zG; + + final double rioAx = (rioCos * rawX - rioSin * rawY) * G_TO_MPS2; + final double rioAy = (rioSin * rawX + rioCos * rawY) * G_TO_MPS2; + final double rioAz = rawZ * G_TO_MPS2; + + final double rioJx = (rioAx - prevRioAx) / Constants.loopPeriodSecs; + final double rioJy = (rioAy - prevRioAy) / Constants.loopPeriodSecs; + final double rioJz = (rioAz - prevRioAz) / Constants.loopPeriodSecs; + + // Acceleration from previous loop + prevRioAx = rioAx; + prevRioAy = rioAy; + prevRioAz = rioAz; + + // IMU rotate is also compute-only (already primitives) + final double imuAx = (imuCos * imuInputs.linearAccelX - imuSin * imuInputs.linearAccelY); + final double imuAy = (imuSin * imuInputs.linearAccelX + imuCos * imuInputs.linearAccelY); + final double imuAz = imuInputs.linearAccelZ; + + final double imuJx = (imuCos * imuInputs.jerkX - imuSin * imuInputs.jerkY); + final double imuJy = (imuSin * imuInputs.jerkX + imuCos * imuInputs.jerkY); + final double imuJz = imuInputs.jerkZ; + + // Fill accel arrays (still math) + rioAccelArr[0] = rioAx; + rioAccelArr[1] = rioAy; + rioAccelArr[2] = rioAz; + imuAccelArr[0] = imuAx; + imuAccelArr[1] = imuAy; + imuAccelArr[2] = imuAz; + + final boolean doHeavyLogs = (++loopCount >= LOG_EVERY_N); + if (doHeavyLogs) { + loopCount = 0; + rioJerkArr[0] = rioJx; + rioJerkArr[1] = rioJy; + rioJerkArr[2] = rioJz; + imuJerkArr[0] = imuJx; + imuJerkArr[1] = imuJy; + imuJerkArr[2] = imuJz; } - prevRioAccel = rioAccVector; + // Logging + Logger.recordOutput("Accel/Rio/Accel_mps2", rioAccelArr); + Logger.recordOutput("Accel/IMU/Accel_mps2", imuAccelArr); + + if (doHeavyLogs) { + Logger.recordOutput("Accel/Rio/Jerk_mps3", rioJerkArr); + Logger.recordOutput("Accel/IMU/Jerk_mps3", imuJerkArr); - long t1 = System.nanoTime(); - Logger.recordOutput("Loop/Accel/total_ms", (t1 - t0) / 1e6); + final double[] ts = imuInputs.odometryYawTimestamps; + if (ts.length > 0) { + Logger.recordOutput("IMU/OdometryLatencySec", Timer.getFPGATimestamp() - ts[ts.length - 1]); + } + } } } diff --git a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java new file mode 100644 index 00000000..64fad53a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIO.java @@ -0,0 +1,26 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.accelerometer; + +public interface RioAccelIO { + final class Inputs { + public boolean connected; + public long timestampNs; + public double xG, yG, zG; // raw in g + } + + default void updateInputs(Inputs inputs) {} + + default void close() {} +} diff --git a/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java new file mode 100644 index 00000000..6cf6ef33 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/accelerometer/RioAccelIORoboRIO.java @@ -0,0 +1,53 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.accelerometer; + +import edu.wpi.first.wpilibj.BuiltInAccelerometer; +import edu.wpi.first.wpilibj.Notifier; + +public class RioAccelIORoboRIO implements RioAccelIO { + private final BuiltInAccelerometer accel = new BuiltInAccelerometer(); + + private volatile long stampNs = 0L; + private volatile double xG = 0.0, yG = 0.0, zG = 0.0; + + private final Notifier sampler; + + public RioAccelIORoboRIO(double sampleHz) { + sampler = + new Notifier( + () -> { + xG = accel.getX(); + yG = accel.getY(); + zG = accel.getZ(); + stampNs = System.nanoTime(); + }); + sampler.setName("RioAccelSampler"); + sampler.startPeriodic(1.0 / sampleHz); + } + + @Override + public void updateInputs(Inputs inputs) { + inputs.connected = true; + inputs.timestampNs = stampNs; + inputs.xG = xG; + inputs.yG = yG; + inputs.zG = zG; + } + + @Override + public void close() { + sampler.stop(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 16c06ef3..fada2245 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -29,7 +29,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -43,7 +42,6 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.AutoConstants; @@ -53,13 +51,13 @@ import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIParsing; -import java.util.Arrays; +import frc.robot.util.RBSISubsystem; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class Drive extends SubsystemBase { +public class Drive extends RBSISubsystem { static final Lock odometryLock = new ReentrantLock(); private final Imu imu; @@ -218,38 +216,34 @@ public Drive(Imu imu) { /** Periodic function that is called each robot cycle by the command scheduler */ @Override - public void periodic() { - final long t0 = System.nanoTime(); + public void rbsiPeriodic() { odometryLock.lock(); - final long t1 = System.nanoTime(); - final var imuInputs = imu.getInputs(); - final long t2 = System.nanoTime(); + // Get the IMU inputs + final var imuInputs = imu.getInputs(); // primitive inputs // Stop modules & log empty setpoint states if disabled if (DriverStation.isDisabled()) { for (var module : modules) { module.stop(); - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } - final long t3 = System.nanoTime(); // Module periodic updates, which drains queues this cycle for (var module : modules) { module.periodic(); } - final long t4 = System.nanoTime(); // Feed historical samples into odometry if REAL robot if (Constants.getMode() != Mode.SIM) { - double[] sampleTimestamps = modules[0].getOdometryTimestamps(); - int sampleCount = sampleTimestamps.length; + final double[] sampleTimestamps = modules[0].getOdometryTimestamps(); + final int sampleCount = sampleTimestamps.length; - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + // Reuse arrays to reduce GC (you likely already have lastModulePositions as a field) + final SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + final SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; for (int i = 0; i < sampleCount; i++) { for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { @@ -262,37 +256,24 @@ public void periodic() { lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; } - // Update gyro angle for odometry - Rotation2d yaw = - imuInputs.connected && imuInputs.odometryYawPositions.length > i - ? imuInputs.odometryYawPositions[i] - : imuInputs.yawPosition; + // Pick yaw sample if available; otherwise fall back to current yaw + final double yawRad = + (imuInputs.connected + && imuInputs.odometryYawPositionsRad != null + && imuInputs.odometryYawPositionsRad.length > i) + ? imuInputs.odometryYawPositionsRad[i] + : imuInputs.yawPositionRad; + + // Boundary conversion: PoseEstimator requires Rotation2d + final Rotation2d yaw = Rotation2d.fromRadians(yawRad); // Apply to pose estimator m_PoseEstimator.updateWithTime(sampleTimestamps[i], yaw, modulePositions); } + Logger.recordOutput("Drive/Pose", m_PoseEstimator.getEstimatedPosition()); } - final long t5 = System.nanoTime(); - odometryLock.unlock(); - final long t6 = System.nanoTime(); - - Logger.recordOutput("Loop/Drive/total_ms", (t6 - t0) / 1e6); - Logger.recordOutput("Loop/Drive/lockWait_ms", (t1 - t0) / 1e6); - Logger.recordOutput("Loop/Drive/getImuInputs_ms", (t2 - t1) / 1e6); - Logger.recordOutput("Loop/Drive/disabled_ms", (t3 - t2) / 1e6); - Logger.recordOutput("Loop/Drive/modules_ms", (t4 - t3) / 1e6); - Logger.recordOutput("Loop/Drive/odometry_ms", (t5 - t4) / 1e6); - Logger.recordOutput("Loop/Drive/unlock_ms", (t6 - t5) / 1e6); - - double driveMs = (t6 - t0) / 1e6; - Logger.recordOutput("Loop/Drive/total_ms", driveMs); - - if (driveMs > 20.0) { - Logger.recordOutput("LoopSpike/Drive/odometry_ms", (t4 - t3) / 1e6); - Logger.recordOutput("LoopSpike/Drive/modules_ms", (t5 - t4) / 1e6); - } gyroDisconnectedAlert.set(!imuInputs.connected && Constants.getMode() != Mode.SIM); } @@ -303,46 +284,54 @@ public void simulationPeriodic() { final double dt = Constants.loopPeriodSecs; // 1) Advance module wheel physics - for (Module module : modules) { - module.simulationPeriodic(); + for (int i = 0; i < modules.length; i++) { + modules[i].simulationPeriodic(); } - // 2) Get module states from modules (authoritative) - SwerveModuleState[] moduleStates = - Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); + // 2) Get module states from modules (authoritative) - NO STREAMS + final SwerveModuleState[] moduleStates = new SwerveModuleState[modules.length]; + for (int i = 0; i < modules.length; i++) { + moduleStates[i] = modules[i].getState(); + } // 3) Update SIM physics (linear + angular) simPhysics.update(moduleStates, dt); - // 4) Feed IMU from authoritative physics - imu.simulationSetYaw(simPhysics.getYaw()); - imu.simulationSetOmega(simPhysics.getOmegaRadPerSec()); - imu.setLinearAccel( - new Translation3d( - simPhysics.getLinearAccel().getX(), simPhysics.getLinearAccel().getY(), 0.0)); + // 4) Feed IMU from authoritative physics (primitive-only boundary) + final double yawRad = + simPhysics.getYaw().getRadians(); // or simPhysics.getYawRad() if you have it + final double omegaRadPerSec = simPhysics.getOmegaRadPerSec(); + + final double ax = simPhysics.getLinearAccel().getX(); + final double ay = simPhysics.getLinearAccel().getY(); + + imu.simulationSetYawRad(yawRad); + imu.simulationSetOmegaRadPerSec(omegaRadPerSec); + imu.simulationSetLinearAccelMps2(ax, ay, 0.0); // 5) Feed PoseEstimator with authoritative yaw and module positions - SwerveModulePosition[] modulePositions = - Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); + // (PoseEstimator still wants objects -> boundary conversion stays here) + final SwerveModulePosition[] modulePositions = new SwerveModulePosition[modules.length]; + for (int i = 0; i < modules.length; i++) { + modulePositions[i] = modules[i].getPosition(); + } m_PoseEstimator.resetPosition( - simPhysics.getYaw(), // gyro reading (authoritative) - modulePositions, // wheel positions - simPhysics.getPose() // pose is authoritative - ); + Rotation2d.fromRadians(yawRad), modulePositions, simPhysics.getPose()); // 6) Optional: inject vision measurement in SIM if (simulatedVisionAvailable) { - Pose2d visionPose = getSimulatedVisionPose(); - double visionTimestamp = Timer.getFPGATimestamp(); - var visionStdDevs = getSimulatedVisionStdDevs(); + final Pose2d visionPose = getSimulatedVisionPose(); + final double visionTimestamp = Timer.getFPGATimestamp(); + final var visionStdDevs = getSimulatedVisionStdDevs(); m_PoseEstimator.addVisionMeasurement(visionPose, visionTimestamp, visionStdDevs); } // 7) Logging Logger.recordOutput("Sim/Pose", simPhysics.getPose()); - Logger.recordOutput("Sim/Yaw", simPhysics.getYaw()); - Logger.recordOutput("Sim/LinearAccel", simPhysics.getLinearAccel()); + Logger.recordOutput("Sim/YawRad", yawRad); + Logger.recordOutput("Sim/OmegaRadPerSec", simPhysics.getOmegaRadPerSec()); + Logger.recordOutput("Sim/LinearAccelXY_mps2", new double[] {ax, ay}); } /** Drive Base Action Functions ****************************************** */ @@ -480,7 +469,7 @@ public Rotation2d getHeading() { if (Constants.getMode() == Mode.SIM) { return simPhysics.getYaw(); } - return imu.getInputs().yawPosition; + return imu.getYaw(); } /** Returns an array of module translations. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index bac7c403..5c68481c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -100,6 +100,7 @@ public class ModuleIOBlended implements ModuleIO { // Inputs from drive motor private final StatusSignal drivePosition; + private final StatusSignal drivePositionOdom; private final Queue drivePositionQueue; private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; @@ -287,8 +288,8 @@ public ModuleIOBlended(int module) { // Create drive status signals drivePosition = driveTalon.getPosition(); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon.getPosition()); + drivePositionOdom = drivePosition.clone(); // NEW + drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePositionOdom); driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getStatorCurrent(); @@ -300,8 +301,11 @@ public ModuleIOBlended(int module) { turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(cancoder.getPosition()); // Configure periodic frames - BaseStatusSignal.setUpdateFrequencyForAll(SwerveConstants.kOdometryFrequency, drivePosition); - BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, driveAppliedVolts, driveCurrent); + BaseStatusSignal.setUpdateFrequencyForAll( + SwerveConstants.kOdometryFrequency, drivePositionOdom); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + ParentDevice.optimizeBusUtilizationForAll(driveTalon); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 45818586..6aec53eb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -247,6 +247,8 @@ public ModuleIOTalonFX(int module) { SwerveConstants.kOdometryFrequency, drivePositionOdom, turnPositionOdom); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, + drivePosition, + turnPosition, driveVelocity, driveAppliedVolts, driveCurrent, @@ -262,17 +264,22 @@ public ModuleIOTalonFX(int module) { public void updateInputs(ModuleIOInputs inputs) { // Refresh most signals - long a0 = System.nanoTime(); - var driveStatus = BaseStatusSignal.refreshAll(driveVelocity, driveAppliedVolts, driveCurrent); - long a1 = System.nanoTime(); - var turnStatus = BaseStatusSignal.refreshAll(turnVelocity, turnAppliedVolts, turnCurrent); - long a2 = System.nanoTime(); + var driveStatus = + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + var turnStatus = + BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); var encStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); - long a3 = System.nanoTime(); - Logger.recordOutput("LoopSpike/Module" + module + "/refresh_drive_ms", (a1 - a0) / 1e6); - Logger.recordOutput("LoopSpike/Module" + module + "/refresh_turn_ms", (a2 - a1) / 1e6); - Logger.recordOutput("LoopSpike/Module" + module + "/refresh_enc_ms", (a3 - a2) / 1e6); + // Log *which* groups are failing and what the code is + if (!driveStatus.isOK()) { + Logger.recordOutput("CAN/Module" + module + "/DriveRefreshStatus", driveStatus.toString()); + } + if (!turnStatus.isOK()) { + Logger.recordOutput("CAN/Module" + module + "/TurnRefreshStatus", turnStatus.toString()); + } + if (!encStatus.isOK()) { + Logger.recordOutput("CAN/Module" + module + "/EncRefreshStatus", encStatus.toString()); + } // Update drive inputs inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); @@ -302,12 +309,7 @@ public void updateInputs(ModuleIOInputs inputs) { .map((Double value) -> Rotation2d.fromRotations(value)) .toArray(Rotation2d[]::new); - // Log queue size right before we clear - Logger.recordOutput("LoopSpike/Module" + module + "/queue_ts_len", timestampQueue.size()); - Logger.recordOutput( - "LoopSpike/Module" + module + "/queue_drive_len", drivePositionQueue.size()); - Logger.recordOutput("LoopSpike/Module" + module + "/queue_turn_len", turnPositionQueue.size()); - + // Clear the queues timestampQueue.clear(); drivePositionQueue.clear(); turnPositionQueue.clear(); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index a0b6caae..23f2885c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -90,6 +90,7 @@ public class ModuleIOTalonFXS implements ModuleIO { // Inputs from drive motor private final StatusSignal drivePosition; + private final StatusSignal drivePositionOdom; private final Queue drivePositionQueue; private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; @@ -98,6 +99,7 @@ public class ModuleIOTalonFXS implements ModuleIO { // Inputs from turn motor private final StatusSignal turnAbsolutePosition; private final StatusSignal turnPosition; + private final StatusSignal turnPositionOdom; private final Queue turnPositionQueue; private final StatusSignal turnVelocity; private final StatusSignal turnAppliedVolts; @@ -234,24 +236,30 @@ public ModuleIOTalonFXS( // Create drive status signals drivePosition = driveTalon.getPosition(); - drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePosition.clone()); + drivePositionOdom = drivePosition.clone(); // NEW + drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePositionOdom); + driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getStatorCurrent(); // Create turn status signals - turnAbsolutePosition = candi.getPWM1Position(); turnPosition = turnTalon.getPosition(); - turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPosition.clone()); + turnPositionOdom = turnPosition.clone(); // NEW + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPositionOdom); + + turnAbsolutePosition = candi.getPWM1Position(); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); turnCurrent = turnTalon.getStatorCurrent(); - // Configure periodic frames + // Configure periodic frames (IMPORTANT: apply odometry rate to the *odom clones*) BaseStatusSignal.setUpdateFrequencyForAll( - SwerveConstants.kOdometryFrequency, drivePosition, turnPosition); + SwerveConstants.kOdometryFrequency, drivePositionOdom, turnPositionOdom); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, + drivePosition, + turnPosition, driveVelocity, driveAppliedVolts, driveCurrent, @@ -259,6 +267,7 @@ public ModuleIOTalonFXS( turnVelocity, turnAppliedVolts, turnCurrent); + ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 09bc9fe2..75eac9d0 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -21,6 +21,7 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -42,6 +43,9 @@ public class PhoenixOdometryThread extends Thread { private static boolean isCANFD = TunerConstants.kCANBus.isNetworkFD(); private static PhoenixOdometryThread instance = null; + private long droppedSamples = 0; + private long loopCount = 0; + public static PhoenixOdometryThread getInstance() { if (instance == null) { instance = new PhoenixOdometryThread(); @@ -63,7 +67,7 @@ public void start() { /** Registers a Phoenix signal to be read from the thread. */ public Queue registerSignal(StatusSignal signal) { - Queue queue = new ArrayBlockingQueue<>(20); + Queue queue = new ArrayBlockingQueue<>(128); // was 20 signalsLock.lock(); Drive.odometryLock.lock(); try { @@ -81,7 +85,7 @@ public Queue registerSignal(StatusSignal signal) { /** Registers a generic signal to be read from the thread. */ public Queue registerSignal(DoubleSupplier signal) { - Queue queue = new ArrayBlockingQueue<>(20); + Queue queue = new ArrayBlockingQueue<>(128); // was 20 signalsLock.lock(); Drive.odometryLock.lock(); try { @@ -144,17 +148,22 @@ public void run() { // Add new samples to queues for (int i = 0; i < phoenixSignals.length; i++) { - phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble()); + 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()); } for (int i = 0; i < timestampQueues.size(); i++) { - timestampQueues.get(i).offer(timestamp); + if (!timestampQueues.get(i).offer(timestamp)) droppedSamples++; } } finally { Drive.odometryLock.unlock(); } + + // every ~1s + if ((loopCount++ % (int) SwerveConstants.kOdometryFrequency) == 0) { + Logger.recordOutput("Drive/OdomThread/DroppedSamples", droppedSamples); + } } } } diff --git a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java index ec377d4e..0a5d052a 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkOdometryThread.java @@ -55,7 +55,7 @@ public void start() { /** Registers a Spark signal to be read from the thread. */ public Queue registerSignal(SparkBase spark, DoubleSupplier signal) { - Queue queue = new ArrayBlockingQueue<>(20); + Queue queue = new ArrayBlockingQueue<>(128); // was 20 Drive.odometryLock.lock(); try { sparks.add(spark); @@ -69,7 +69,7 @@ public Queue registerSignal(SparkBase spark, DoubleSupplier signal) { /** Registers a generic signal to be read from the thread. */ public Queue registerSignal(DoubleSupplier signal) { - Queue queue = new ArrayBlockingQueue<>(20); + Queue queue = new ArrayBlockingQueue<>(128); // was 20 Drive.odometryLock.lock(); try { genericSignals.add(signal); diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java index c69a06bd..2a7a98f4 100644 --- a/src/main/java/frc/robot/subsystems/imu/Imu.java +++ b/src/main/java/frc/robot/subsystems/imu/Imu.java @@ -19,55 +19,75 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; -import frc.robot.util.VirtualSubsystem; -import org.littletonrobotics.junction.Logger; - -public class Imu extends VirtualSubsystem { +public class Imu { private final ImuIO io; - private final ImuIOInputsAutoLogged inputs = new ImuIOInputsAutoLogged(); + private final ImuIO.ImuIOInputs inputs = new ImuIO.ImuIOInputs(); + + // Optional per-cycle cached objects (avoid repeated allocations) + private long cacheStampNs = -1L; + private Rotation2d cachedYaw = Rotation2d.kZero; + private Translation3d cachedAccel = Translation3d.kZero; + private Translation3d cachedJerk = Translation3d.kZero; public Imu(ImuIO io) { this.io = io; } - @Override - protected void rbsiPeriodic() { - final long t0 = System.nanoTime(); + public void periodic() { io.updateInputs(inputs); - final long t1 = System.nanoTime(); - Logger.processInputs("IMU", inputs); - final long t2 = System.nanoTime(); - - Logger.recordOutput("Loop/IMU/update_ms", (t1 - t0) / 1e6); - Logger.recordOutput("Loop/IMU/log_ms", (t2 - t1) / 1e6); - Logger.recordOutput("Loop/IMU/total_ms", (t2 - t0) / 1e6); - - double totalMs = (t2 - t0) / 1e6; - Logger.recordOutput("Loop/IMU/total_ms", totalMs); - if (totalMs > 2.0) { - Logger.recordOutput("LoopSpike/IMU/update_ms", (t1 - t0) / 1e6); - } } - public ImuIOInputsAutoLogged getInputs() { + /** Hot-path access: primitive-only snapshot */ + public ImuIO.ImuIOInputs getInputs() { return inputs; } - // Pass-throughs so Drive can still control the IMU + /** Readable boundary: Rotation2d (alloc/cached per timestamp) */ + public Rotation2d getYaw() { + refreshCachesIfNeeded(); + return cachedYaw; + } + + /** Readable boundary: Translation3d accel (alloc/cached per timestamp) */ + public Translation3d getLinearAccel() { + refreshCachesIfNeeded(); + return cachedAccel; + } + + public Translation3d getJerk() { + refreshCachesIfNeeded(); + return cachedJerk; + } + public void zeroYaw(Rotation2d yaw) { - io.zeroYaw(yaw); + io.zeroYawRad(yaw.getRadians()); + } + + private void refreshCachesIfNeeded() { + final long stamp = inputs.timestampNs; + if (stamp == cacheStampNs) return; + cacheStampNs = stamp; + + cachedYaw = Rotation2d.fromRadians(inputs.yawPositionRad); + cachedAccel = new Translation3d(inputs.linearAccelX, inputs.linearAccelY, inputs.linearAccelZ); + cachedJerk = new Translation3d(inputs.jerkX, inputs.jerkY, inputs.jerkZ); } - public void simulationSetYaw(Rotation2d yaw) { - io.simulationSetYaw(yaw); + // ---------------- SIM PUSH (primitive-only boundary) ---------------- + + /** Simulation: push authoritative yaw (radians) into the IO layer */ + public void simulationSetYawRad(double yawRad) { + io.simulationSetYawRad(yawRad); } - public void simulationSetOmega(double omegaRadPerSec) { - io.simulationSetOmega(omegaRadPerSec); + /** Simulation: push authoritative yaw rate (rad/s) into the IO layer */ + public void simulationSetOmegaRadPerSec(double omegaRadPerSec) { + io.simulationSetOmegaRadPerSec(omegaRadPerSec); } - public void setLinearAccel(Translation3d accelMps2) { - io.setLinearAccel(accelMps2); + /** Simulation: push authoritative linear accel (m/s^2) into the IO layer */ + public void simulationSetLinearAccelMps2(double ax, double ay, double az) { + io.simulationSetLinearAccelMps2(ax, ay, az); } } diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index d8c05f92..37aa695f 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -17,51 +17,63 @@ package frc.robot.subsystems.imu; -import static edu.wpi.first.units.Units.*; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.util.RBSIIO; import org.littletonrobotics.junction.AutoLog; /** - * Single IMU interface exposing all relevant state: orientation, rates, linear accel, and odometry. + * Single IMU interface exposing all relevant state: orientation, rates, linear accel, jerk, and + * odometry samples. + * + *

Primitive-only core: NO WPILib geometry objects, NO Units objects. Conversions happen at the + * boundary in the Imu subsystem (wrapper methods). */ public interface ImuIO extends RBSIIO { @AutoLog - public static class ImuIOInputs { + class ImuIOInputs { public boolean connected = false; - // Timestamp - public long timestampNs = 0; + /** FPGA-local timestamp when inputs were captured (ns) */ + public long timestampNs = 0L; + + /** Yaw angle (robot frame) in radians */ + public double yawPositionRad = 0.0; + + /** Yaw angular rate in radians/sec */ + public double yawRateRadPerSec = 0.0; - // Gyro - public Rotation2d yawPosition = Rotation2d.kZero; - public AngularVelocity yawVelocityRadPerSec = RadiansPerSecond.of(0.0); + /** Linear acceleration in robot frame (m/s^2) */ + public double linearAccelX = 0.0; - // Linear acceleration and jerk in robot frame (m/s^2; m/s^3) - public Translation3d linearAccel = Translation3d.kZero; - public Translation3d jerk = Translation3d.kZero; + public double linearAccelY = 0.0; + public double linearAccelZ = 0.0; + /** Linear jerk in robot frame (m/s^3) */ + public double jerkX = 0.0; + + public double jerkY = 0.0; + public double jerkZ = 0.0; + + /** Time spent in the IO update call (seconds) */ public double latencySeconds = 0.0; - // Odometry tracking (optional, for external odometry/vision fusion) + /** Optional odometry samples (timestamps in seconds) */ public double[] odometryYawTimestamps = new double[] {}; - public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + + /** Optional odometry samples (yaw positions in radians) */ + public double[] odometryYawPositionsRad = new double[] {}; } /** Update the current IMU readings into `inputs` */ default void updateInputs(ImuIOInputs inputs) {} - /** Zero the yaw to a known field-relative angle */ - default void zeroYaw(Rotation2d yaw) {} + /** Zero the yaw to a known field-relative angle (radians) */ + default void zeroYawRad(double yawRad) {} - /** Simulation-only update hooks */ - default void simulationSetYaw(Rotation2d yaw) {} + /** Simulation-only hooks */ + default void simulationSetYawRad(double yawRad) {} - default void simulationSetOmega(double omegaRadPerSec) {} + default void simulationSetOmegaRadPerSec(double omegaRadPerSec) {} - default void setLinearAccel(Translation3d accelMps2) {} + default void simulationSetLinearAccelMps2(double ax, double ay, double az) {} } diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java index 2c7e2fd9..4bf1edcd 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIONavX.java @@ -17,84 +17,162 @@ package frc.robot.subsystems.imu; -import static edu.wpi.first.units.Units.*; - import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.subsystems.drive.PhoenixOdometryThread; import frc.robot.subsystems.drive.SwerveConstants; +import java.util.Iterator; import java.util.Queue; -/** - * IMU IO for NavX. Provides yaw, angular velocity, acceleration, odometry, and latency for - * AdvantageKit logging. - */ +/** IMU IO for NavX. Primitive-only: yaw/rate in radians, accel in m/s^2, jerk in m/s^3. */ public class ImuIONavX implements ImuIO { + private static final double DEG_TO_RAD = Math.PI / 180.0; + private static final double G_TO_MPS2 = 9.80665; private final AHRS navx; - private final Queue yawPositionQueue; + + // Phoenix odometry queues (boxed Doubles, but we drain without streams) + private final Queue yawPositionDegQueue; private final Queue yawTimestampQueue; - private Translation3d prevAccel = Translation3d.kZero; + // Previous accel (m/s^2) for jerk + private double prevAx = 0.0, prevAy = 0.0, prevAz = 0.0; + private long prevTimestampNs = 0L; + + // Reusable buffers for queue drain + private double[] odomTsBuf = new double[8]; + private double[] odomYawRadBuf = new double[8]; public ImuIONavX() { // Initialize NavX over SPI navx = new AHRS(NavXComType.kMXP_SPI, (byte) SwerveConstants.kOdometryFrequency); - // Zero based on alliance - if (DriverStation.getAlliance().get() == Alliance.Blue) { - navx.setAngleAdjustment(0.0); - } else { + // Alliance-based adjustment (your original behavior) + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red) { navx.setAngleAdjustment(180.0); + } else { + navx.setAngleAdjustment(0.0); } navx.reset(); - // Register queues for odometry replay/logging yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(navx::getYaw); + yawPositionDegQueue = PhoenixOdometryThread.getInstance().registerSignal(navx::getYaw); } @Override public void updateInputs(ImuIOInputs inputs) { - long start = System.nanoTime(); + final long start = System.nanoTime(); inputs.connected = navx.isConnected(); - inputs.yawPosition = Rotation2d.fromDegrees(-navx.getAngle()); - inputs.yawVelocityRadPerSec = RadiansPerSecond.of(-navx.getRawGyroZ()); - inputs.linearAccel = - new Translation3d( - navx.getWorldLinearAccelX(), - navx.getWorldLinearAccelY(), - navx.getWorldLinearAccelZ()) - .times(9.81); // Convert to m/s^2 - // Compute the jerk and set the new timestamp - double timediff = (start - inputs.timestampNs) / 1.0e9; - inputs.jerk = inputs.linearAccel.minus(prevAccel).div(timediff); + + // Your original sign convention: + // yawPosition = Rotation2d.fromDegrees(-navx.getAngle()); + // yawRate = -navx.getRawGyroZ() + // + // NavX: + // - getAngle() is degrees (continuous) + // - getRawGyroZ() is deg/sec + final double yawDeg = -navx.getAngle(); + final double yawRateDegPerSec = -navx.getRawGyroZ(); + + inputs.yawPositionRad = yawDeg * DEG_TO_RAD; + inputs.yawRateRadPerSec = yawRateDegPerSec * DEG_TO_RAD; + + // World linear accel (NavX returns "g" typically). Convert to m/s^2. + final double ax = navx.getWorldLinearAccelX() * G_TO_MPS2; + final double ay = navx.getWorldLinearAccelY() * G_TO_MPS2; + final double az = navx.getWorldLinearAccelZ() * G_TO_MPS2; + + inputs.linearAccelX = ax; + inputs.linearAccelY = ay; + inputs.linearAccelZ = az; + + // Jerk + if (prevTimestampNs != 0L) { + final double dt = (start - prevTimestampNs) * 1e-9; + if (dt > 1e-6) { + final double invDt = 1.0 / dt; + inputs.jerkX = (ax - prevAx) * invDt; + inputs.jerkY = (ay - prevAy) * invDt; + inputs.jerkZ = (az - prevAz) * invDt; + } + } + + prevTimestampNs = start; + prevAx = ax; + prevAy = ay; + prevAz = az; + inputs.timestampNs = start; - // Update odometry history - inputs.odometryYawTimestamps = yawTimestampQueue.stream().mapToDouble(d -> d).toArray(); - inputs.odometryYawPositions = - yawPositionQueue.stream().map(d -> Rotation2d.fromDegrees(-d)).toArray(Rotation2d[]::new); + // Odometry history + final int n = drainOdomQueues(); + if (n > 0) { + final double[] tsOut = new double[n]; + final double[] yawOut = new double[n]; + System.arraycopy(odomTsBuf, 0, tsOut, 0, n); + System.arraycopy(odomYawRadBuf, 0, yawOut, 0, n); + inputs.odometryYawTimestamps = tsOut; + inputs.odometryYawPositionsRad = yawOut; + } else { + inputs.odometryYawTimestamps = new double[] {}; + inputs.odometryYawPositionsRad = new double[] {}; + } - // Latency in seconds - long end = System.nanoTime(); - inputs.latencySeconds = (end - start) * 1.0e-9; + final long end = System.nanoTime(); + inputs.latencySeconds = (end - start) * 1e-9; } - /** - * Zero the NavX - * - * @param yaw The yaw to which to reset the gyro - */ @Override - public void zeroYaw(Rotation2d yaw) { - navx.setAngleAdjustment(yaw.getDegrees()); + public void zeroYawRad(double yawRad) { + navx.setAngleAdjustment(yawRad / DEG_TO_RAD); navx.zeroYaw(); + + // Reset jerk history so you don't spike on the next frame + prevTimestampNs = 0L; + prevAx = prevAy = prevAz = 0.0; + } + + private int drainOdomQueues() { + final int nTs = yawTimestampQueue.size(); + final int nYaw = yawPositionDegQueue.size(); + final int n = Math.min(nTs, nYaw); + if (n <= 0) { + yawTimestampQueue.clear(); + yawPositionDegQueue.clear(); + return 0; + } + + ensureOdomCapacity(n); + + final Iterator itT = yawTimestampQueue.iterator(); + final Iterator itY = yawPositionDegQueue.iterator(); + + int i = 0; + while (i < n && itT.hasNext() && itY.hasNext()) { + odomTsBuf[i] = itT.next(); + + // queue provides degrees (navx::getYaw). Apply your sign convention (-d) then rad. + final double yawDeg = -itY.next(); + odomYawRadBuf[i] = yawDeg * DEG_TO_RAD; + + i++; + } + + yawTimestampQueue.clear(); + yawPositionDegQueue.clear(); + return i; + } + + private void ensureOdomCapacity(int n) { + if (odomTsBuf.length >= n) return; + int newCap = odomTsBuf.length; + while (newCap < n) newCap *= 2; + odomTsBuf = new double[newCap]; + odomYawRadBuf = new double[newCap]; } // /** diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java index 7aef3faa..0e07f02f 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java @@ -17,108 +17,163 @@ package frc.robot.subsystems.imu; -import static edu.wpi.first.units.Units.*; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearAcceleration; import frc.robot.subsystems.drive.PhoenixOdometryThread; import frc.robot.subsystems.drive.SwerveConstants; import frc.robot.util.RBSICANBusRegistry; +import java.util.Iterator; import java.util.Queue; -/** IMU IO for CTRE Pigeon2 */ +/** IMU IO for CTRE Pigeon2 (primitive-only hot path) */ public class ImuIOPigeon2 implements ImuIO { + private static final double DEG_TO_RAD = Math.PI / 180.0; + private static final double G_TO_MPS2 = 9.80665; + private final Pigeon2 pigeon = new Pigeon2( SwerveConstants.kPigeonId, RBSICANBusRegistry.getBus(SwerveConstants.kCANbusName)); + + // Cached signals private final StatusSignal yawSignal = pigeon.getYaw(); private final StatusSignal yawRateSignal = pigeon.getAngularVelocityZWorld(); + + private final StatusSignal accelX = pigeon.getAccelerationX(); + private final StatusSignal accelY = pigeon.getAccelerationY(); + private final StatusSignal accelZ = pigeon.getAccelerationZ(); + private final Queue odomTimestamps; - private final Queue odomYaws; + private final Queue odomYawsDeg; - private Translation3d prevAccel = Translation3d.kZero; + // Previous accel for jerk (m/s^2) + private double prevAx = 0.0, prevAy = 0.0, prevAz = 0.0; + private long prevTimestampNs = 0L; + + // Reusable buffers for queue-drain (avoid streams) + private double[] odomTsBuf = new double[8]; + private double[] odomYawRadBuf = new double[8]; - /** Constructor */ public ImuIOPigeon2() { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); + yawSignal.setUpdateFrequency(SwerveConstants.kOdometryFrequency); yawRateSignal.setUpdateFrequency(50.0); + + accelX.setUpdateFrequency(50.0); + accelY.setUpdateFrequency(50.0); + accelZ.setUpdateFrequency(50.0); + pigeon.optimizeBusUtilization(); - // Create queues for odometry logging/replay inside the class odomTimestamps = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - odomYaws = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw()); + odomYawsDeg = PhoenixOdometryThread.getInstance().registerSignal(yawSignal); } - /** Update inputs for logging and robot code */ @Override public void updateInputs(ImuIOInputs inputs) { - long start = System.nanoTime(); - - inputs.connected = BaseStatusSignal.refreshAll(yawSignal, yawRateSignal).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yawSignal.getValueAsDouble()); - inputs.yawVelocityRadPerSec = DegreesPerSecond.of(yawRateSignal.getValueAsDouble()); - - inputs.linearAccel = - new Translation3d( - pigeon.getAccelerationX().getValueAsDouble(), - pigeon.getAccelerationY().getValueAsDouble(), - pigeon.getAccelerationZ().getValueAsDouble()) - .times(9.81); // Convert to m/s^2 - - // Compute the jerk and set the new timestamp - double timediff = (start - inputs.timestampNs) / 1.0e9; - inputs.jerk = inputs.linearAccel.minus(prevAccel).div(timediff); - inputs.timestampNs = start; - - inputs.odometryYawTimestamps = odomTimestamps.stream().mapToDouble(d -> d).toArray(); - inputs.odometryYawPositions = - odomYaws.stream().map(deg -> Rotation2d.fromDegrees(deg)).toArray(Rotation2d[]::new); + final long start = System.nanoTime(); + + StatusCode code = BaseStatusSignal.refreshAll(yawSignal, yawRateSignal, accelX, accelY, accelZ); + inputs.connected = code.isOK(); + + // Yaw / rate: Phoenix returns degrees and deg/s here; convert to radians + final double yawDeg = yawSignal.getValueAsDouble(); + final double yawRateDegPerSec = yawRateSignal.getValueAsDouble(); + + inputs.yawPositionRad = yawDeg * DEG_TO_RAD; + inputs.yawRateRadPerSec = yawRateDegPerSec * DEG_TO_RAD; + + // Accel: Phoenix returns "g" for these signals (common for Pigeon2). Convert to m/s^2 + final double ax = accelX.getValueAsDouble() * G_TO_MPS2; + final double ay = accelY.getValueAsDouble() * G_TO_MPS2; + final double az = accelZ.getValueAsDouble() * G_TO_MPS2; + + inputs.linearAccelX = ax; + inputs.linearAccelY = ay; + inputs.linearAccelZ = az; + + // Jerk from delta accel / dt + if (prevTimestampNs != 0L) { + final double dt = (start - prevTimestampNs) * 1e-9; + if (dt > 1e-6) { + final double invDt = 1.0 / dt; + inputs.jerkX = (ax - prevAx) * invDt; + inputs.jerkY = (ay - prevAy) * invDt; + inputs.jerkZ = (az - prevAz) * invDt; + } + } + + prevTimestampNs = start; + prevAx = ax; + prevAy = ay; + prevAz = az; - odomTimestamps.clear(); - odomYaws.clear(); + inputs.timestampNs = start; - // Latency in seconds - long end = System.nanoTime(); - inputs.latencySeconds = (end - start) * 1.0e-9; + // Drain odometry queues to primitive arrays (timestamps are already doubles; yaws are degrees) + final int n = drainOdometryQueuesIntoBuffers(); + if (n > 0) { + final double[] tsOut = new double[n]; + final double[] yawOut = new double[n]; + System.arraycopy(odomTsBuf, 0, tsOut, 0, n); + System.arraycopy(odomYawRadBuf, 0, yawOut, 0, n); + inputs.odometryYawTimestamps = tsOut; + inputs.odometryYawPositionsRad = yawOut; + } else { + inputs.odometryYawTimestamps = new double[] {}; + inputs.odometryYawPositionsRad = new double[] {}; + } + + final long end = System.nanoTime(); + inputs.latencySeconds = (end - start) * 1e-9; } - /** - * Zero the Pigeon2 - * - * @param yaw The yaw to which to reset the gyro - */ @Override - public void zeroYaw(Rotation2d yaw) { - pigeon.setYaw(yaw.getDegrees()); + public void zeroYawRad(double yawRad) { + pigeon.setYaw(yawRad / DEG_TO_RAD); } - // /** - // * Zero the Pigeon2 - // * - // *

This method should always rezero the pigeon in ALWAYS-BLUE-ORIGIN orientation. Testing, - // * however, shows that it's not doing what I think it should be doing. There is likely - // * interference with something else in the odometry - // */ - // @Override - // public void zero() { - // // With the Pigeon facing forward, forward depends on the alliance selected. - // if (DriverStation.getAlliance().get() == Alliance.Blue) { - // System.out.println("Alliance Blue: Setting YAW to 0"); - // pigeon.setYaw(0.0); - // } else { - // System.out.println("Alliance Red: Setting YAW to 180"); - // pigeon.setYaw(180.0); - // } - // } + private int drainOdometryQueuesIntoBuffers() { + final int nTs = odomTimestamps.size(); + final int nYaw = odomYawsDeg.size(); + final int n = Math.min(nTs, nYaw); + if (n <= 0) { + odomTimestamps.clear(); + odomYawsDeg.clear(); + return 0; + } + + ensureOdomCapacity(n); + + // Iterate without streams (still boxed because Queue, but avoids stream overhead) + final Iterator itT = odomTimestamps.iterator(); + final Iterator itY = odomYawsDeg.iterator(); + + int i = 0; + while (i < n && itT.hasNext() && itY.hasNext()) { + odomTsBuf[i] = itT.next(); + odomYawRadBuf[i] = itY.next() * DEG_TO_RAD; + i++; + } + odomTimestamps.clear(); + odomYawsDeg.clear(); + return i; + } + + private void ensureOdomCapacity(int n) { + if (odomTsBuf.length >= n) return; + int newCap = odomTsBuf.length; + while (newCap < n) newCap *= 2; + odomTsBuf = new double[newCap]; + odomYawRadBuf = new double[newCap]; + } } diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java index 10a7b6d9..080dec1c 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOSim.java @@ -17,77 +17,107 @@ package frc.robot.subsystems.imu; -import static edu.wpi.first.units.Units.RadiansPerSecond; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.Timer; -import java.util.LinkedList; -import java.util.Queue; import org.littletonrobotics.junction.Logger; -/** Simulated IMU for full robot simulation & replay logging */ +/** Simulated IMU for full robot simulation & replay logging (primitive-only) */ public class ImuIOSim implements ImuIO { + private static final double RAD_TO_DEG = 180.0 / Math.PI; - // --- AUTHORITATIVE SIM STATE --- - private Rotation2d yaw = Rotation2d.kZero; + // --- AUTHORITATIVE SIM STATE (PRIMITIVES) --- + private double yawRad = 0.0; private double yawRateRadPerSec = 0.0; - private Translation3d linearAccel = Translation3d.kZero; + private double ax = 0.0, ay = 0.0, az = 0.0; // m/s^2 - // --- ODOMETRY HISTORY FOR LOGGING/LATENCY --- - private final Queue odomTimestamps = new LinkedList<>(); - private final Queue odomYaws = new LinkedList<>(); + // --- ODOMETRY HISTORY (PRIMITIVE RING BUFFER) --- + private static final int ODOM_CAP = 50; + private final double[] odomTs = new double[ODOM_CAP]; + private final double[] odomYawRad = new double[ODOM_CAP]; + private int odomSize = 0; + private int odomHead = 0; // next write index public ImuIOSim() {} // ---------------- SIMULATION INPUTS (PUSH) ---------------- - /** Set yaw from authoritative physics */ - public void simulationSetYaw(Rotation2d yaw) { - this.yaw = yaw; + @Override + public void simulationSetYawRad(double yawRad) { + this.yawRad = yawRad; } - /** Set angular velocity from authoritative physics */ - public void simulationSetOmega(double omegaRadPerSec) { + @Override + public void simulationSetOmegaRadPerSec(double omegaRadPerSec) { this.yawRateRadPerSec = omegaRadPerSec; } - /** Set linear acceleration from physics (optional) */ - public void setLinearAccel(Translation3d accelMps2) { - this.linearAccel = accelMps2; + @Override + public void simulationSetLinearAccelMps2(double ax, double ay, double az) { + this.ax = ax; + this.ay = ay; + this.az = az; } // ---------------- IO UPDATE (PULL) ---------------- - /** Populate the IMUIOInputs object with the current SIM state */ @Override public void updateInputs(ImuIOInputs inputs) { inputs.connected = true; - - // Authoritative physics - inputs.yawPosition = yaw; - inputs.yawVelocityRadPerSec = RadiansPerSecond.of(yawRateRadPerSec); - inputs.linearAccel = linearAccel; - - // Maintain odometry history for logging / latency purposes - double now = Timer.getFPGATimestamp(); - odomTimestamps.add(now); - odomYaws.add(yaw); - - while (odomTimestamps.size() > 50) odomTimestamps.poll(); - while (odomYaws.size() > 50) odomYaws.poll(); - - inputs.odometryYawTimestamps = odomTimestamps.stream().mapToDouble(d -> d).toArray(); - inputs.odometryYawPositions = odomYaws.toArray(Rotation2d[]::new); - - // Logging for SIM analysis - Logger.recordOutput("IMU/Yaw", yaw); - Logger.recordOutput("IMU/YawRateDps", Math.toDegrees(yawRateRadPerSec)); + inputs.timestampNs = System.nanoTime(); + + // Authoritative sim state + inputs.yawPositionRad = yawRad; + inputs.yawRateRadPerSec = yawRateRadPerSec; + inputs.linearAccelX = ax; + inputs.linearAccelY = ay; + inputs.linearAccelZ = az; + + // Jerk: SIM doesn’t have a prior accel here unless you want it; set to 0 by default. + // If you do want jerk, you can add prevAx/prevAy/prevAz + dt just like the real IO. + inputs.jerkX = 0.0; + inputs.jerkY = 0.0; + inputs.jerkZ = 0.0; + + // Maintain odometry history + pushOdomSample(Timer.getFPGATimestamp(), yawRad); + + // Export odometry arrays (copy out in chronological order) + final int n = odomSize; + final double[] tsOut = new double[n]; + final double[] yawOut = new double[n]; + + // Oldest sample index: + int idx = (odomHead - odomSize); + while (idx < 0) idx += ODOM_CAP; + + for (int i = 0; i < n; i++) { + tsOut[i] = odomTs[idx]; + yawOut[i] = odomYawRad[idx]; + idx++; + if (idx == ODOM_CAP) idx = 0; + } + + inputs.odometryYawTimestamps = tsOut; + inputs.odometryYawPositionsRad = yawOut; + + // Optional: SIM logging (primitive-friendly) + Logger.recordOutput("IMU/YawRad", yawRad); + Logger.recordOutput("IMU/YawDeg", yawRad * RAD_TO_DEG); + Logger.recordOutput("IMU/YawRateDps", yawRateRadPerSec * RAD_TO_DEG); } @Override - public void zeroYaw(Rotation2d yaw) { - this.yaw = yaw; + public void zeroYawRad(double yawRad) { + this.yawRad = yawRad; this.yawRateRadPerSec = 0.0; } + + private void pushOdomSample(double timestampSec, double yawRad) { + odomTs[odomHead] = timestampSec; + odomYawRad[odomHead] = yawRad; + + odomHead++; + if (odomHead == ODOM_CAP) odomHead = 0; + + if (odomSize < ODOM_CAP) odomSize++; + } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 68183f1c..263ed34b 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -23,14 +23,14 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.Cameras; import frc.robot.FieldConstants; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; +import frc.robot.util.VirtualSubsystem; import java.util.ArrayList; import org.littletonrobotics.junction.Logger; -public class Vision extends SubsystemBase { +public class Vision extends VirtualSubsystem { private final VisionConsumer consumer; private final VisionIO[] io; private final VisionIOInputsAutoLogged[] inputs; @@ -69,7 +69,10 @@ public Rotation2d getTargetX(int cameraIndex) { } @Override - public void periodic() { + public void rbsiPeriodic() {} + + public void somethingElse() { + // Update inputs + process inputs first (cheap, and keeps AK logs consistent) for (int i = 0; i < io.length; i++) { io[i].updateInputs(inputs[i]); diff --git a/src/main/java/frc/robot/util/RBSICANBusRegistry.java b/src/main/java/frc/robot/util/RBSICANBusRegistry.java index f247cde0..b0707fc6 100644 --- a/src/main/java/frc/robot/util/RBSICANBusRegistry.java +++ b/src/main/java/frc/robot/util/RBSICANBusRegistry.java @@ -1,3 +1,16 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + package frc.robot.util; import com.ctre.phoenix6.CANBus; diff --git a/src/main/java/frc/robot/util/RBSICANHealth.java b/src/main/java/frc/robot/util/RBSICANHealth.java index 901d6632..df7a185a 100644 --- a/src/main/java/frc/robot/util/RBSICANHealth.java +++ b/src/main/java/frc/robot/util/RBSICANHealth.java @@ -29,8 +29,8 @@ protected void rbsiPeriodic() { var status = bus.getStatus(); Logger.recordOutput("CAN/" + bus.getName() + "/Utilization", status.BusUtilization); Logger.recordOutput("CAN/" + bus.getName() + "/TxFullCount", status.TxFullCount); - Logger.recordOutput("CAN/" + bus.getName() + "/REC", status.REC); - Logger.recordOutput("CAN/" + bus.getName() + "/TEC", status.TEC); + Logger.recordOutput("CAN/" + bus.getName() + "/RxErrorCount", status.REC); + Logger.recordOutput("CAN/" + bus.getName() + "/TxErrorCount", status.TEC); Logger.recordOutput("CAN/" + bus.getName() + "/BusOffCount", status.BusOffCount); } }