From 8166c11e2b871d2121c942b7b5b18f1f200ac95b Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Thu, 8 Oct 2026 00:17:55 -0500 Subject: [PATCH 1/4] subsystem implementation of the rotor motor --- src/main/java/frc/robot/Constants.java | 15 +++ .../robot/subsystems/shooter/rotor/Rotor.java | 68 ++++++++++++ .../subsystems/shooter/rotor/RotorIO.java | 47 ++++++++ .../subsystems/shooter/rotor/RotorIOReal.java | 93 ++++++++++++++++ .../subsystems/shooter/rotor/RotorIOSim.java | 104 ++++++++++++++++++ 5 files changed, 327 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/rotor/RotorIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8ccfaec..c2d14cc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,6 +13,7 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Value; import static edu.wpi.first.units.Units.Volts; import java.util.List; @@ -207,4 +208,18 @@ public static class SimulationConstants public static final Distance EXTENDED_DISTANCE = Inches.of(10); public static final Distance RETRACTED_DISTANCE = Inches.of(0); } + + public static class ShooterConstants + { + // Rotor + public static final Current ROTOR_CURRENT_LIMIT = Amps.of(80); + public static final Dimensionless ROTOR_GEAR_RATIO = Value.of(36).div(Value.of(1)); // 36:1 + public static final double ROTOR_KG = 0; + public static final double ROTOR_KS = 0; + public static final double ROTOR_KV = 0; + public static final double ROTOR_KA = 0; + public static final double ROTOR_KP = 0; + public static final double ROTOR_KI = 0; + public static final double ROTOR_KD = 0; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java b/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java new file mode 100644 index 0000000..3ab99a8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java @@ -0,0 +1,68 @@ +package frc.robot.subsystems.shooter.rotor; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.rotor.RotorIO.RotorIOInputs; + +/* + * Class containing the business logic for how the rotor component of the + * shooter subsystem works + */ +@Logged +public class Rotor extends SubsystemBase +{ + // The inputs measured from the rotor motor + @Logged + private final RotorIOInputs _inputs; + + // The physical layer (motors and sensors) the business logic interacts with + @NotLogged + private final RotorIO _io; + + // Creates an instance of a rotor + public Rotor() + { + // Inputs is just a container for variables. It can always be initialized using + // the default constructor + _inputs = new RotorIOInputs(); + + // Instantiate an appropriate physical layer object based on if we're running on + // a real robot or in simulation + if (RobotBase.isReal()) + { + _io = new RotorIOReal(); + } + else + { + _io = new RotorIOSim(); + } + } + + // Executes every 20ms. Used to measure inputs from the physical layer + @Override + public void periodic() + { + _io.updateInputs(_inputs); + } + + // Gets the current rate of rotation of the rotor motor + public AngularVelocity getRate() + { + return _inputs.rotationRate; + } + + // Sets the desired angular rate of rotation of the rotor + public void setRate(AngularVelocity rate) + { + _io.setRate(rate); + } + + // Commands the rotor to stop rotating + public void stop() + { + _io.stop(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIO.java b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIO.java new file mode 100644 index 0000000..cc1be09 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIO.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.shooter.rotor; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + +/* + * Interface for objects that represent the physical layer (motors and sensors) + * of the rotor sub-component of the shooter subsystem. + */ +public interface RotorIO +{ + /* + * A container for the sensor inputs we can expect back from the rotor motor + * each timestep. + */ + @Logged + public static class RotorIOInputs + { + // The voltage the rotor is currently applying + @Logged + public Voltage appliedVoltage = Volts.zero(); + + // The amount of current the rotor is currently drawing + @Logged + public Current currentDraw = Amps.zero(); + + // The current rotational velocity of the rotor + @Logged + public AngularVelocity rotationRate = RPM.zero(); + } + + // Allows the RotorIO object to read sensor inputs. Updated values are + // loaded into the provided "inputs" parameter. + public void updateInputs(RotorIOInputs inputs); + + // Sets the desired angular rate of rotation of the rotor + public void setRate(AngularVelocity rate); + + // Commands the rotor to stop rotating + public void stop(); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java new file mode 100644 index 0000000..718ef18 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java @@ -0,0 +1,93 @@ +package frc.robot.subsystems.shooter.rotor; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Value; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants.CANConstants; +import frc.robot.Constants.ShooterConstants; + +/* + * A real implementation of the physical layer (motors and sensors) of the rotor + * sub-component of the shooter subsystem. + */ +public class RotorIOReal implements RotorIO +{ + // The rotor motor + private final TalonFX _rotorMotor; + + // The request object for running the rotor at a specified velocity + private final VelocityVoltage _velocityRequest; + + // Constructs a RotorIOReal instance + public RotorIOReal() + { + // Initialize the motor + _rotorMotor = new TalonFX(CANConstants.ROTOR_MOTOR); + + // Set up the velocity request for rotating at a desired rate + _velocityRequest = new VelocityVoltage(RPM.zero()).withSlot(0); + + // Start configuring the motor + var config = new TalonFXConfiguration(); + + // Motor configurations related to current limits + config.CurrentLimits.StatorCurrentLimit = ShooterConstants.ROTOR_CURRENT_LIMIT.in(Amps); + config.CurrentLimits.StatorCurrentLimitEnable = true; + + // Motor configurations related to general motor output + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + // Motor configurations related to closed loop gains + config.Slot0.kG = ShooterConstants.ROTOR_KG; + config.Slot0.kS = ShooterConstants.ROTOR_KS; + config.Slot0.kV = ShooterConstants.ROTOR_KV; + config.Slot0.kA = ShooterConstants.ROTOR_KA; + config.Slot0.kP = ShooterConstants.ROTOR_KP; + config.Slot0.kI = ShooterConstants.ROTOR_KI; + config.Slot0.kD = ShooterConstants.ROTOR_KD; + + // Set the gear ratio of the motor + config.Feedback.SensorToMechanismRatio = ShooterConstants.ROTOR_GEAR_RATIO.in(Value); + + // Apply the configuration to the motor + _rotorMotor.getConfigurator().apply(config); + } + + // Allows the RotorIOReal object to read sensor inputs. Updated + // values are loaded into the provided "inputs" parameter. + public void updateInputs(RotorIOInputs inputs) + { + // Save off the new inputs as measured by the motor + inputs.appliedVoltage = _rotorMotor.getMotorVoltage().getValue(); + inputs.currentDraw = _rotorMotor.getStatorCurrent().getValue(); + inputs.rotationRate = _rotorMotor.getVelocity().getValue(); + } + + // Sets the desired angular rate of rotation of the rotor + public void setRate(AngularVelocity rate) + { + // If a non-positive speed is requested, turn off the rotor + if (rate.lte(RPM.zero())) + { + stop(); + return; + } + + _rotorMotor.setControl(_velocityRequest.withVelocity(rate)); + } + + // Commands the rotor to stop rotating + public void stop() + { + _rotorMotor.setVoltage(0); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java new file mode 100644 index 0000000..d0a7277 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java @@ -0,0 +1,104 @@ +package frc.robot.subsystems.shooter.rotor; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Value; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Constants.GeneralConstants; +import frc.robot.Constants.ShooterConstants; + +/* + * A simulated implementation of the physical layer (motors and sensors) of the + * rotor sub-component of the shooter subsystem. + */ +public class RotorIOSim implements RotorIO +{ + // The mathematical model of the rotor motor + private final DCMotor _rotorModel; + + // The simulation object for the rotor. The rotor is essentially a sideways + // flywheel + private final FlywheelSim _rotorSim; + + // The feed forward model for velocity control + private final SimpleMotorFeedforward _feedForward; + + // The pid controller for velocity control + private final PIDController _pid; + + // The target velocity + private AngularVelocity _targetVelocity; + + // Constructs a RotorIOSim instance + public RotorIOSim() + { + // Initialize the mathematical model + _rotorModel = DCMotor.getKrakenX60(1); + + // Initialize the simulation object + _rotorSim = new FlywheelSim(LinearSystemId.createFlywheelSystem(_rotorModel, 0.01, ShooterConstants.ROTOR_GEAR_RATIO.in(Value)), _rotorModel, null); + + // Initialize feed forward for velocity control + _feedForward = new SimpleMotorFeedforward(ShooterConstants.ROTOR_KS, ShooterConstants.ROTOR_KV, ShooterConstants.ROTOR_KA); + + // Initialize pid for velocity control + _pid = new PIDController(ShooterConstants.ROTOR_KP, ShooterConstants.ROTOR_KI, ShooterConstants.ROTOR_KD); + + // Initialize target velocity. Off by default + _targetVelocity = RPM.zero(); + } + + // Allows the RotorIOReal object to read sensor inputs. Updated + // values are loaded into the provided "inputs" parameter. + public void updateInputs(RotorIOInputs inputs) + { + // Calculate the voltage to apply to the rotor + var volts = Volts.zero(); + + // Only calculate the feedforward and feedback if our target velocity is above + // zero + if (_targetVelocity.gt(RPM.zero())) + { + var feedForward = Volts.of(_feedForward.calculate(_targetVelocity.in(RPM))); + var feedback = Volts.of(_pid.calculate(_rotorSim.getAngularVelocityRPM(), _targetVelocity.in(RPM))); + + volts = feedForward.plus(feedback); + } + + // Set the input voltage for the model and calculate the new inputs + _rotorSim.setInputVoltage(volts.in(Volts)); + _rotorSim.update(GeneralConstants.LOOP_PERIOD.in(Seconds)); + + // Save off the new inputs + inputs.appliedVoltage = Volts.of(_rotorSim.getInputVoltage()); + inputs.currentDraw = Amps.of(_rotorSim.getCurrentDrawAmps()); + inputs.rotationRate = _rotorSim.getAngularVelocity(); + } + + // Sets the desired angular rate of rotation of the rotor + public void setRate(AngularVelocity rate) + { + // If a non-positive speed is requested, turn off the rotor + if (rate.lte(RPM.zero())) + { + stop(); + return; + } + + _targetVelocity = rate; + } + + // Commands the rotor to stop rotating + public void stop() + { + _targetVelocity = RPM.zero(); + } +} From a7376f6a9a21bb7e938045727d01c3a2670b34e1 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Thu, 8 Oct 2026 11:52:30 -0500 Subject: [PATCH 2/4] implement rotor with some light testing --- src/main/java/frc/robot/Constants.java | 21 +++++++----- src/main/java/frc/robot/RobotContainer.java | 11 ++++++ .../robot/subsystems/shooter/rotor/Rotor.java | 34 +++++++++++++++++++ .../subsystems/shooter/rotor/RotorIOReal.java | 17 ++++++---- .../subsystems/shooter/rotor/RotorIOSim.java | 8 +++-- 5 files changed, 72 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c2d14cc..b7c0918 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,6 +12,8 @@ import static edu.wpi.first.units.Units.Percent; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Value; import static edu.wpi.first.units.Units.Volts; @@ -29,7 +31,10 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.AngularAccelerationUnit; +import edu.wpi.first.units.AngularVelocityUnit; import edu.wpi.first.units.DistanceUnit; +import edu.wpi.first.units.VoltageUnit; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -212,14 +217,12 @@ public static class SimulationConstants public static class ShooterConstants { // Rotor - public static final Current ROTOR_CURRENT_LIMIT = Amps.of(80); - public static final Dimensionless ROTOR_GEAR_RATIO = Value.of(36).div(Value.of(1)); // 36:1 - public static final double ROTOR_KG = 0; - public static final double ROTOR_KS = 0; - public static final double ROTOR_KV = 0; - public static final double ROTOR_KA = 0; - public static final double ROTOR_KP = 0; - public static final double ROTOR_KI = 0; - public static final double ROTOR_KD = 0; + public static final Current ROTOR_CURRENT_LIMIT = Amps.of(80); + public static final Dimensionless ROTOR_GEAR_RATIO = Value.of(36).div(Value.of(1)); // 36:1 + public static final Voltage ROTOR_KS = Volts.of(0.0); + public static final Per ROTOR_KV = Volts.of(12.0).div(RPM.of(6000.0).div(ROTOR_GEAR_RATIO)); + public static final Per ROTOR_KA = Volts.of(0.0).per(RotationsPerSecondPerSecond); + public static final Per ROTOR_KP = Volts.of(0.025).per(RPM); + public static final Per ROTOR_KD = Volts.of(0.00005).per(RPM.per(Second)); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b63e203..6bdd36f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Value; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; @@ -19,6 +20,7 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.TunerConstants; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.rotor.Rotor; import frc.robot.util.MeasureUtil; @Logged @@ -33,6 +35,9 @@ public class RobotContainer private final Autos _autos = new Autos(_drive, _intake); // private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; + // Shooter subsystem components + private final Rotor _rotor = new Rotor(); + public RobotContainer() { configureDefaultCommands(); @@ -80,6 +85,12 @@ private void configureButtonBindings() _operator.rightTrigger().whileTrue(_intake.jiggle()); _operator.povDown().onTrue(_intake.getRetractCmd()); _operator.povUp().onTrue(_intake.getExtendCmd()); + + // Rotor testing commands + _operator.a().onTrue(_rotor.commands.stop()); + _operator.y().onTrue(_rotor.commands.setRate(RPM.of(80))); + _operator.b().whileTrue(_rotor.commands.run(RPM.of(100))); + _operator.x().whileTrue(_rotor.commands.run(RPM.of(60))); } public Command getAutonomousCommand() diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java b/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java index 3ab99a8..9d2bbd6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java @@ -4,6 +4,7 @@ import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.shooter.rotor.RotorIO.RotorIOInputs; @@ -22,6 +23,11 @@ public class Rotor extends SubsystemBase @NotLogged private final RotorIO _io; + // Provides a clean way for other classes to instantiate commands that run on + // this object + @NotLogged + public final RotorCommandFactory commands = new RotorCommandFactory(); + // Creates an instance of a rotor public Rotor() { @@ -65,4 +71,32 @@ public void stop() { _io.stop(); } + + /************ + * Commands * + ************/ + + // Nested helper class responsible for creating commands that act on the + // enclosing Rotor object instance + public class RotorCommandFactory + { + // Creates a command to set the angular rate of rotation of the rotor + public Command setRate(AngularVelocity rate) + { + return runOnce(() -> Rotor.this.setRate(rate)); + } + + // Creates a command to stop the rotation of the rotor + public Command stop() + { + return runOnce(Rotor.this::stop); + } + + // Creates a command to set the angular rate of rotation of the rotor. When the + // command ends, the rotor stops rotating + public Command run(AngularVelocity rate) + { + return startEnd(() -> Rotor.this.setRate(rate), Rotor.this::stop); + } + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java index 718ef18..98c0ce4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java @@ -2,7 +2,10 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Value; +import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -47,13 +50,13 @@ public RotorIOReal() config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // Motor configurations related to closed loop gains - config.Slot0.kG = ShooterConstants.ROTOR_KG; - config.Slot0.kS = ShooterConstants.ROTOR_KS; - config.Slot0.kV = ShooterConstants.ROTOR_KV; - config.Slot0.kA = ShooterConstants.ROTOR_KA; - config.Slot0.kP = ShooterConstants.ROTOR_KP; - config.Slot0.kI = ShooterConstants.ROTOR_KI; - config.Slot0.kD = ShooterConstants.ROTOR_KD; + config.Slot0.kG = 0.0; + config.Slot0.kS = ShooterConstants.ROTOR_KS.in(Volts); + config.Slot0.kV = ShooterConstants.ROTOR_KV.in(Volts.per(RPM)); + config.Slot0.kA = ShooterConstants.ROTOR_KA.in(Volts.per(RotationsPerSecondPerSecond)); + config.Slot0.kP = ShooterConstants.ROTOR_KP.in(Volts.per(RPM)); + config.Slot0.kI = 0.0; + config.Slot0.kD = ShooterConstants.ROTOR_KD.in(Volts.per(RPM.per(Second))); // Set the gear ratio of the motor config.Feedback.SensorToMechanismRatio = ShooterConstants.ROTOR_GEAR_RATIO.in(Value); diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java index d0a7277..a0e14fd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java @@ -2,6 +2,8 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Value; import static edu.wpi.first.units.Units.Volts; @@ -44,13 +46,13 @@ public RotorIOSim() _rotorModel = DCMotor.getKrakenX60(1); // Initialize the simulation object - _rotorSim = new FlywheelSim(LinearSystemId.createFlywheelSystem(_rotorModel, 0.01, ShooterConstants.ROTOR_GEAR_RATIO.in(Value)), _rotorModel, null); + _rotorSim = new FlywheelSim(LinearSystemId.createFlywheelSystem(_rotorModel, 0.1, ShooterConstants.ROTOR_GEAR_RATIO.in(Value)), _rotorModel); // Initialize feed forward for velocity control - _feedForward = new SimpleMotorFeedforward(ShooterConstants.ROTOR_KS, ShooterConstants.ROTOR_KV, ShooterConstants.ROTOR_KA); + _feedForward = new SimpleMotorFeedforward(ShooterConstants.ROTOR_KS.in(Volts), ShooterConstants.ROTOR_KV.in(Volts.per(RPM)), ShooterConstants.ROTOR_KA.in(Volts.per(RotationsPerSecondPerSecond))); // Initialize pid for velocity control - _pid = new PIDController(ShooterConstants.ROTOR_KP, ShooterConstants.ROTOR_KI, ShooterConstants.ROTOR_KD); + _pid = new PIDController(ShooterConstants.ROTOR_KP.in(Volts.per(RPM)), 0.0, ShooterConstants.ROTOR_KD.in(Volts.per(RPM.per(Second)))); // Initialize target velocity. Off by default _targetVelocity = RPM.zero(); From dfa722980451094b91bf440e24805aeb1d60a10f Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Thu, 8 Oct 2026 16:17:47 -0500 Subject: [PATCH 3/4] move rotor business logic for rate handling out of physical layer into subsystem --- .../java/frc/robot/subsystems/shooter/rotor/Rotor.java | 9 +++++++++ .../frc/robot/subsystems/shooter/rotor/RotorIOReal.java | 7 ------- .../frc/robot/subsystems/shooter/rotor/RotorIOSim.java | 7 ------- 3 files changed, 9 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java b/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java index 9d2bbd6..d8d73d9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.shooter.rotor; +import static edu.wpi.first.units.Units.RPM; + import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.units.measure.AngularVelocity; @@ -63,6 +65,13 @@ public AngularVelocity getRate() // Sets the desired angular rate of rotation of the rotor public void setRate(AngularVelocity rate) { + // If a non-positive rate is requested, turn off the rotor + if (rate.lte(RPM.zero())) + { + stop(); + return; + } + _io.setRate(rate); } diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java index 98c0ce4..03c03a4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java @@ -78,13 +78,6 @@ public void updateInputs(RotorIOInputs inputs) // Sets the desired angular rate of rotation of the rotor public void setRate(AngularVelocity rate) { - // If a non-positive speed is requested, turn off the rotor - if (rate.lte(RPM.zero())) - { - stop(); - return; - } - _rotorMotor.setControl(_velocityRequest.withVelocity(rate)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java index a0e14fd..f199c6a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java @@ -88,13 +88,6 @@ public void updateInputs(RotorIOInputs inputs) // Sets the desired angular rate of rotation of the rotor public void setRate(AngularVelocity rate) { - // If a non-positive speed is requested, turn off the rotor - if (rate.lte(RPM.zero())) - { - stop(); - return; - } - _targetVelocity = rate; } From b02e516cd9184e30e580c1f52d18ec5ebb63c4f4 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Thu, 8 Oct 2026 23:50:04 -0500 Subject: [PATCH 4/4] fix comment in simulation class --- .../java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java index f199c6a..8975ae8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java @@ -58,7 +58,7 @@ public RotorIOSim() _targetVelocity = RPM.zero(); } - // Allows the RotorIOReal object to read sensor inputs. Updated + // Allows the RotorIOSim object to read sensor inputs. Updated // values are loaded into the provided "inputs" parameter. public void updateInputs(RotorIOInputs inputs) {