diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8ccfaec..b7c0918 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,7 +12,10 @@ 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; import java.util.List; @@ -28,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; @@ -207,4 +213,16 @@ 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 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 new file mode 100644 index 0000000..d8d73d9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/Rotor.java @@ -0,0 +1,111 @@ +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; +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; + +/* + * 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; + + // 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() + { + // 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) + { + // If a non-positive rate is requested, turn off the rotor + if (rate.lte(RPM.zero())) + { + stop(); + return; + } + + _io.setRate(rate); + } + + // Commands the rotor to stop rotating + 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/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..03c03a4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOReal.java @@ -0,0 +1,89 @@ +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.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; +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 = 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); + + // 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) + { + _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..8975ae8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/rotor/RotorIOSim.java @@ -0,0 +1,99 @@ +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.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; + +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.1, ShooterConstants.ROTOR_GEAR_RATIO.in(Value)), _rotorModel); + + // Initialize feed forward for velocity control + _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.in(Volts.per(RPM)), 0.0, ShooterConstants.ROTOR_KD.in(Volts.per(RPM.per(Second)))); + + // Initialize target velocity. Off by default + _targetVelocity = RPM.zero(); + } + + // Allows the RotorIOSim 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) + { + _targetVelocity = rate; + } + + // Commands the rotor to stop rotating + public void stop() + { + _targetVelocity = RPM.zero(); + } +}