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/13] 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 be7ab97..35242ee 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 9755675..2eee8a6 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/13] 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 9e33d5d..c8f0802 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 ce35545..624d078 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 59ecd1b..2102b49 --- 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 4bd9017..97d4194 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 10ff019..8d46b80 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 a8470d8..b9614d2 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 9e101cd..5a80997 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 61201dd..57e89d5 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 9a79f9f..1542f85 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/13] 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 c8f0802..27d0849 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 8d46b80..d902948 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 35242ee..8b934be 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/13] 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 2102b49..3e23fec 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/13] 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 27d0849..82a7913 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 83d5b19..0ebfbe5 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 624d078..8bb188e 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 3419d5e..80901f8 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 9b538e1..0a2a5e7 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 91129b7..0a1435e 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 97d4194..abc40ee 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/13] 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 82a7913..d7a382e 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 0a1435e..286a863 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/13] 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 d7a382e..92c4536 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 8b934be..80406e5 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 c1b6901..47fa874 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 d65eade..a8e8c20 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 cef38ce..ee89859 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 04391d6..99dfb2c 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/13] 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 99dfb2c..1b964f5 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/13] 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 8bb188e..531c4ee 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/13] 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 92c4536..5d4111c 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 0ebfbe5..d39bafa 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 7039b86..4629b6a 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 d902948..6319902 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 c2fdc35..7573861 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 8f47a65..73270a7 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 b9614d2..b616b0b 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 5a80997..34bdf72 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 57e89d5..98f775f 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 d954f6b..664dafb 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 6b71f49..68183f1 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 13f93b3..f8d9cdd 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 54972cf..4d84b8f 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 2eee8a6..803ae00 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 18005ff..e51ccba 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 534aed0..8db7f56 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/13] 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 531c4ee..adef129 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 4629b6a..3981b7b 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 286a863..5ac22d5 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 0000000..370b355 --- /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 9273f0c..d8c05f9 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 8db7f56..53f9d6e 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/13] 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 d39bafa..e9034e2 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 3981b7b..7752dd4 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 5ac22d5..a539b53 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 370b355..c69a06b 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 803ae00..3aa3b8b 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 53f9d6e..2996aa3 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/13] 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 5d4111c..cfb7214 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 d8e516d..10bcd05 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 a539b53..16c06ef 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 6319902..bac7c40 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 73270a7..fc76b59 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 b616b0b..4581858 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 34bdf72..a0b6caa 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 188f987..09bc9fe 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 c97fc50..ec377d4 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 664dafb..7aef3fa 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 0000000..f247cde --- /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 0000000..901d663 --- /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 d01e87d..ba6daad 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 ---