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 1/8] 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 2/8] 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 3/8] 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 4/8] 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 5/8] 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 6/8] 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 7/8] 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 8/8] 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