From 2eb114eaf8f998a5e119cbf234f38df17d578a09 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Thu, 8 Oct 2026 16:15:17 -0500 Subject: [PATCH 1/3] basic layout for flywheel --- src/main/java/frc/robot/Constants.java | 9 ++ .../subsystems/shooter/flywheel/Flywheel.java | 131 ++++++++++++++++++ .../shooter/flywheel/FlywheelIO.java | 59 ++++++++ .../shooter/flywheel/FlywheelIOReal.java | 27 ++++ .../shooter/flywheel/FlywheelIOSim.java | 27 ++++ 5 files changed, 253 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOReal.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b7c0918..dd609ac 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -224,5 +224,14 @@ public static class ShooterConstants 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)); + + // Flywheel + public static final Current FLYWHEEL_CURRENT_LIMIT = Amps.of(80); + public static final Voltage FLYWHEEL_KS = Volts.of(0.0); + public static final Per FLYWHEEL_KV = Volts.of(0.0).div(RPM.of(0.0)); + public static final Per FLYWHEEL_KA = Volts.of(0.0).per(RotationsPerSecondPerSecond); + public static final Per FLYWHEEL_KP = Volts.of(0.0).per(RPM); + public static final Per FLYWHEEL_KD = Volts.of(0.0).per(RPM.per(Second)); + public static final AngularVelocity FLYWHEEL_TOLERANCE = RPM.of(50); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java new file mode 100644 index 0000000..de18356 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java @@ -0,0 +1,131 @@ +package frc.robot.subsystems.shooter.flywheel; + +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; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ShooterConstants; +import frc.robot.subsystems.shooter.flywheel.FlywheelIO.FlywheelIOInputs; + +/* + * Class containing the business logic for how the flywheel component of the + * shooter subsystem works + */ +@Logged +public class Flywheel extends SubsystemBase +{ + // The inputs measured from the flywheel + @Logged + private final FlywheelIOInputs _inputs; + + // The physical layer (motors and sensors) the business logic interacts with + @NotLogged + private final FlywheelIO _io; + + // The target rate of rotation for the flywheel + @Logged + private AngularVelocity _targetRate; + + // Creates an instance of a flywheel + public Flywheel() + { + // Inputs is just a container for variables. In can always be initialized using + // the default constructor + _inputs = new FlywheelIOInputs(); + + // Instantiate an appropriate physical layer object based on if we're running on + // a real robot or in simulation + if (RobotBase.isReal()) + { + _io = new FlywheelIOReal(); + } + else + { + _io = new FlywheelIOSim(); + } + + // Initialize private non-final member variables + _targetRate = RPM.zero(); + } + + // 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 flywheel + public AngularVelocity getRate() + { + // Since the flywheel is made up of two motors, we'll take the average of both + // motors' speeds. The difference should be negligible since they're + // mechanically linked on the same output shaft. + return _inputs.leftMotorRotationRate.plus(_inputs.rightMotorRotationRate).div(2); + } + + // Gets the current target rate of rotation for the flywheel + public AngularVelocity getTargetRate() + { + return _targetRate; + } + + // Determines if the flywheel is currently spinning at the target angular + // velocity. Only returns true if the flywheel is actually on. + public boolean atSpeed() + { + return _targetRate.gt(RPM.zero()) && getRate().isNear(_targetRate, ShooterConstants.FLYWHEEL_TOLERANCE); + } + + // Sets the target rate of rotation for the flywheel + public void setRate(AngularVelocity rate) + { + if (rate.lte(RPM.zero())) + { + stop(); + return; + } + + _targetRate = rate; + _io.setRate(_targetRate); + } + + // Commands the flywheel to stop rotating + public void stop() + { + _targetRate = RPM.zero(); + _io.stop(); + } + + /************ + * Commands * + ************/ + + // Nested helper class responsible for creating commands that act on the + // enclosing Flywheel object instance + public class FlywheelCommandFactory + { + // Creates a command to set the angular rate of rotation of the flywheel + public Command setRate(AngularVelocity rate) + { + return runOnce(() -> Flywheel.this.setRate(rate)); + } + + // Creates a command to stop the rotation of the flywheel + public Command stop() + { + return runOnce(Flywheel.this::stop); + } + + // Creates a command to set the angular rate of rotation of the flywheel. When + // the command ends, the flywheel stops rotating + public Command run(AngularVelocity rate) + { + return startEnd(() -> Flywheel.this.setRate(rate), Flywheel.this::stop); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java new file mode 100644 index 0000000..7873d8b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.shooter.flywheel; + +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 flywheel sub-component of the shooter subsystem. + */ +public interface FlywheelIO +{ + /* + * A container for the sensor inputs we can expect back from the flywheel each + * timestep. + */ + @Logged + public static class FlywheelIOInputs + { + // The voltage currently applied to the left flywheel motor + @Logged + public Voltage leftMotorAppliedVoltage = Volts.zero(); + + // The amount of current the left flywheel motor is currently drawing + @Logged + public Current leftMotorCurrentDraw = Amps.zero(); + + // The current rotational velocity of the left flywheel motor + @Logged + public AngularVelocity leftMotorRotationRate = RPM.zero(); + + // The voltage currently applied to the right flywheel motor + @Logged + public Voltage rightMotorAppliedVoltage = Volts.zero(); + + // The amound of current the right flywheel motor is currently drawing + @Logged + public Current rightMotorCurrentDraw = Amps.zero(); + + // The current rotational velocity of the right flywheel motor + @Logged + public AngularVelocity rightMotorRotationRate = RPM.zero(); + } + + // Allows the FlywheelIO object to read sensor inputs. Updated values are loaded + // into the provided "inputs" parameter. + public void updateInputs(FlywheelIOInputs inputs); + + // Sets the desired angular rate of rotation of the flywheel + public void setRate(AngularVelocity rate); + + // Commands the flywheel to stop rotating + public void stop(); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOReal.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOReal.java new file mode 100644 index 0000000..69b2b0a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOReal.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.shooter.flywheel; + +import edu.wpi.first.units.measure.AngularVelocity; + +public class FlywheelIOReal implements FlywheelIO +{ + @Override + public void updateInputs(FlywheelIOInputs inputs) + { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'updateInputs'"); + } + + @Override + public void setRate(AngularVelocity rate) + { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setRate'"); + } + + @Override + public void stop() + { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'stop'"); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java new file mode 100644 index 0000000..506ae80 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.shooter.flywheel; + +import edu.wpi.first.units.measure.AngularVelocity; + +public class FlywheelIOSim implements FlywheelIO +{ + @Override + public void updateInputs(FlywheelIOInputs inputs) + { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'updateInputs'"); + } + + @Override + public void setRate(AngularVelocity rate) + { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setRate'"); + } + + @Override + public void stop() + { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'stop'"); + } +} From 5c3ce3405310499dce98f42eb495197663a22ff6 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Thu, 8 Oct 2026 23:50:23 -0500 Subject: [PATCH 2/3] implement physical layer classes --- .../shooter/flywheel/FlywheelIOReal.java | 107 +++++++++++++++++- .../shooter/flywheel/FlywheelIOSim.java | 90 ++++++++++++++- 2 files changed, 185 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOReal.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOReal.java index 69b2b0a..b3b8e43 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOReal.java @@ -1,27 +1,122 @@ package frc.robot.subsystems.shooter.flywheel; +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.Volts; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants.CANConstants; +import frc.robot.Constants.GeneralConstants; +import frc.robot.Constants.ShooterConstants; +/* + * A real implementation of the physical layer (motors and sensors) of the + * flywheel sub-component of the shooter subsystem. + */ public class FlywheelIOReal implements FlywheelIO { + // The left flywheel motor + private final SparkFlex _leftFlywheelMotor; + + // The right flywheel motor + private final SparkFlex _rightFlywheelMotor; + + // The closed loop controller for running the flywheel at a specified velocity + private final SparkClosedLoopController _pid; + + // The encoder built into the left flywheel motor + private final RelativeEncoder _leftFlywheelEncoder; + + // The encoder built into the right flywheel motor + private final RelativeEncoder _rightFlywheelEncoder; + + public FlywheelIOReal() + { + // Instantiate the motors + _leftFlywheelMotor = new SparkFlex(CANConstants.FLYWHEEL_LEAD, MotorType.kBrushless); + _rightFlywheelMotor = new SparkFlex(CANConstants.FLYWHEEL_FOLLOW, MotorType.kBrushless); + + // Set the CAN timeout high for initial configuration + _leftFlywheelMotor.setCANTimeout(250); // Milliseconds + _rightFlywheelMotor.setCANTimeout(250); // Milliseconds + + // Start configuring the motors + var config = new SparkFlexConfig(); + + // Motor configurations related to general motor output + config.inverted(false); + config.idleMode(IdleMode.kCoast); + config.smartCurrentLimit((int)ShooterConstants.FLYWHEEL_CURRENT_LIMIT.in(Amps)); + config.voltageCompensation(GeneralConstants.MOTOR_VOLTAGE.in(Volts)); + + // Motor configurations related to closed loop gains + config.closedLoop.feedForward.kG(0.0); + config.closedLoop.feedForward.kS(ShooterConstants.FLYWHEEL_KS.in(Volts)); + config.closedLoop.feedForward.kV(ShooterConstants.FLYWHEEL_KV.in(Volts.per(RPM))); + config.closedLoop.feedForward.kA(ShooterConstants.FLYWHEEL_KA.in(Volts.per(RotationsPerSecondPerSecond))); + config.closedLoop.p(ShooterConstants.FLYWHEEL_KP.in(Volts.per(RPM))); + config.closedLoop.i(0.0); + config.closedLoop.d(ShooterConstants.FLYWHEEL_KD.in(Volts.per(RPM.per(Second)))); + + // Configure the leader motor + _leftFlywheelMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + // Provide an additional configuration for the follower motor + config.follow(_leftFlywheelMotor); + + // Configure the follower motor + _rightFlywheelMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + // Get the encoders + _leftFlywheelEncoder = _leftFlywheelMotor.getEncoder(); + _rightFlywheelEncoder = _leftFlywheelMotor.getEncoder(); + + // The closed loop controller + _pid = _leftFlywheelMotor.getClosedLoopController(); + + // Reset the CAN timeouts + _leftFlywheelMotor.setCANTimeout(0); + _rightFlywheelMotor.setCANTimeout(0); + } + + // Allows the FlywheelIOReal object to read sensor inputs. Updated values are + // loaded into the provided "inputs" parameter. @Override public void updateInputs(FlywheelIOInputs inputs) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'updateInputs'"); + inputs.leftMotorAppliedVoltage = Volts.of(_leftFlywheelMotor.getBusVoltage()).times(_leftFlywheelMotor.getAppliedOutput()); + inputs.leftMotorCurrentDraw = Amps.of(_leftFlywheelMotor.getOutputCurrent()); + inputs.leftMotorRotationRate = RPM.of(_leftFlywheelEncoder.getVelocity()); + + inputs.rightMotorAppliedVoltage = Volts.of(_rightFlywheelMotor.getBusVoltage()).times(_rightFlywheelMotor.getAppliedOutput()); + inputs.rightMotorCurrentDraw = Amps.of(_rightFlywheelMotor.getOutputCurrent()); + inputs.rightMotorRotationRate = RPM.of(_rightFlywheelEncoder.getVelocity()); } + // Sets the desired angular rate of rotation of the flywheel. @Override public void setRate(AngularVelocity rate) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setRate'"); + _pid.setSetpoint(rate.in(RPM), ControlType.kVelocity, ClosedLoopSlot.kSlot0); } + // Commands the flywheel to stop rotating. @Override public void stop() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'stop'"); + _leftFlywheelMotor.setVoltage(Volts.zero()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java index 506ae80..601432c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -1,27 +1,105 @@ package frc.robot.subsystems.shooter.flywheel; +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.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 + */ public class FlywheelIOSim implements FlywheelIO { + // The mathematical model of the flywheel motors. + private final DCMotor _flywheelModel; + + // The simulation object for the flywheel. + private final FlywheelSim _flywheelSim; + + // 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 FlywheelIOSim instance. + public FlywheelIOSim() + { + // Initialize the mathematical model + _flywheelModel = DCMotor.getNeoVortex(2); + + // Initialize the simulation object + _flywheelSim = new FlywheelSim(LinearSystemId.createFlywheelSystem(_flywheelModel, 0.001, 1), _flywheelModel); + + // Initialize feed forward for velocity control + _feedForward = new SimpleMotorFeedforward(ShooterConstants.FLYWHEEL_KS.in(Volts), ShooterConstants.FLYWHEEL_KV.in(Volts.per(RPM)), ShooterConstants.FLYWHEEL_KA.in(Volts.per(RotationsPerSecondPerSecond))); + + // Initialize the pid for velocity control + _pid = new PIDController(ShooterConstants.FLYWHEEL_KP.in(Volts.per(RPM)), 0.0, ShooterConstants.FLYWHEEL_KD.in(Volts.per(RPM.per(Second)))); + + // Initialize target velocity. Off by default + _targetVelocity = RPM.zero(); + } + + // Allows the FlywheelIOSim object to read sensor inputs. Updated values are + // loaded into the provided "inputs" parameter. @Override public void updateInputs(FlywheelIOInputs inputs) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'updateInputs'"); + // Calculate the voltage to apply to the flywheel + 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(_flywheelSim.getAngularVelocityRPM(), _targetVelocity.in(RPM))); + + volts = feedForward.plus(feedback); + } + + // Set the input voltage for the model and calculate the new inputs + _flywheelSim.setInputVoltage(volts.in(Volts)); + _flywheelSim.update(GeneralConstants.LOOP_PERIOD.in(Seconds)); + + // Save off the new inputs + inputs.leftMotorAppliedVoltage = Volts.of(_flywheelSim.getInputVoltage()); + inputs.leftMotorCurrentDraw = Amps.of(_flywheelSim.getCurrentDrawAmps()); + inputs.leftMotorRotationRate = _flywheelSim.getAngularVelocity(); + + // The flywheel plant model measures both motors together. For simulation, it's + // fine to assume that both motors are doing the exact same thing. + inputs.rightMotorAppliedVoltage = inputs.leftMotorAppliedVoltage; + inputs.rightMotorCurrentDraw = inputs.leftMotorCurrentDraw; + inputs.rightMotorRotationRate = inputs.leftMotorRotationRate; } + // Sets the desired angular rate of rotation of the flywheel. @Override public void setRate(AngularVelocity rate) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setRate'"); + _targetVelocity = rate; } + // Commands the flywheel to stop rotating @Override public void stop() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'stop'"); + _targetVelocity = RPM.zero(); } } From 5ea60a342ff7318008306304079975cb102cc30b Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Fri, 9 Oct 2026 00:07:53 -0500 Subject: [PATCH 3/3] set up flywheel simulation controls --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 14 +++++++------- .../subsystems/shooter/flywheel/Flywheel.java | 5 +++++ 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dd609ac..f81e329 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -228,9 +228,9 @@ public static class ShooterConstants // Flywheel public static final Current FLYWHEEL_CURRENT_LIMIT = Amps.of(80); public static final Voltage FLYWHEEL_KS = Volts.of(0.0); - public static final Per FLYWHEEL_KV = Volts.of(0.0).div(RPM.of(0.0)); + public static final Per FLYWHEEL_KV = Volts.of(12.0).div(RPM.of(6784.0)); public static final Per FLYWHEEL_KA = Volts.of(0.0).per(RotationsPerSecondPerSecond); - public static final Per FLYWHEEL_KP = Volts.of(0.0).per(RPM); + public static final Per FLYWHEEL_KP = Volts.of(0.00015).per(RPM); public static final Per FLYWHEEL_KD = Volts.of(0.0).per(RPM.per(Second)); public static final AngularVelocity FLYWHEEL_TOLERANCE = RPM.of(50); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6bdd36f..bba86ba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,7 +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.subsystems.shooter.flywheel.Flywheel; import frc.robot.util.MeasureUtil; @Logged @@ -36,7 +36,7 @@ public class RobotContainer // private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; // Shooter subsystem components - private final Rotor _rotor = new Rotor(); + private final Flywheel _flywheel = new Flywheel(); public RobotContainer() { @@ -86,11 +86,11 @@ private void configureButtonBindings() _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))); + // Flywheel testing commands + _operator.a().onTrue(_flywheel.commands.stop()); + _operator.y().onTrue(_flywheel.commands.setRate(RPM.of(4800))); + _operator.b().whileTrue(_flywheel.commands.run(RPM.of(5400))); + _operator.x().whileTrue(_flywheel.commands.run(RPM.of(3600))); } public Command getAutonomousCommand() diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java index de18356..503fa15 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/Flywheel.java @@ -26,6 +26,11 @@ public class Flywheel extends SubsystemBase @NotLogged private final FlywheelIO _io; + // Provides a clean way for other classes to instantiate commands that run on + // this object + @NotLogged + public final FlywheelCommandFactory commands = new FlywheelCommandFactory(); + // The target rate of rotation for the flywheel @Logged private AngularVelocity _targetRate;