diff --git a/tasks/lesson3-shooter-task/src/main/java/frc/robot/RobotContainer.java b/tasks/lesson3-shooter-task/src/main/java/frc/robot/RobotContainer.java index 4a1f5c7..6f04ec0 100644 --- a/tasks/lesson3-shooter-task/src/main/java/frc/robot/RobotContainer.java +++ b/tasks/lesson3-shooter-task/src/main/java/frc/robot/RobotContainer.java @@ -10,11 +10,11 @@ public class RobotContainer { private final CommandJoystick joystick = new CommandJoystick(0); - // private final Shooter shooter; + private final Shooter shooter; public RobotContainer() { // Use real or sim ShooterIO - // shooter = new Shooter(RobotBase.isReal() ? new ShooterIOTalonFX() : new ShooterIOSim()); + shooter = new Shooter(RobotBase.isReal() ? new ShooterIOTalonFX() : new ShooterIOSim()); configureBindings(); } @@ -28,5 +28,9 @@ private void configureBindings() { .whileTrue(Commands.run(() -> shooter.spin(0.5), shooter)) .onFalse(Commands.run(() -> shooter.stop(), shooter)); */ + joystick + .button(1) + .whileTrue(Commands.run(() -> shooter.spin(0.5), shooter)) + .onFalse(Commands.run(() -> shooter.stop(), shooter)); } } diff --git a/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 8e170c4..6ded98c 100644 --- a/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,27 +1,39 @@ package frc.robot.subsystems.shooter; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.ShooterIO.ShooterIOInputs; // Main Shooter subsystem logic public class Shooter extends SubsystemBase { - // TODO: Add a private field for your ShooterIO instance - // TODO: Add a private field for your ShooterIOInputs instance and initialize it - - // TODO: Create the constructor that takes a ShooterIO instance as a parameter - // public Shooter(ShooterIO io) { ... } + // Add a private field for your ShooterIO instance + private ShooterIO shooterIO = new ShooterIOTalonFX(); + // Add a private field for your ShooterIOInputs instance and initialize it + private ShooterIOInputs shooterIOInputs = new ShooterIOInputs(); + // Create the constructor that takes a ShooterIO instance as a parameter + public Shooter(ShooterIO io) { + this.shooterIO = io; + } @Override public void periodic() { // This method will be called once per scheduler run - // TODO: Call the updateInputs method on your ShooterIO instance + // Call the updateInputs method on your ShooterIO instance + shooterIO.updateInputs(shooterIOInputs); + System.out.println(shooterIOInputs.motorRPM); } - // TODO: Add a public method to spin the shooter motor at a given percentage output - // public void spin(double percent) { ... } + // Add a public method to spin the shooter motor at a given percentage output + public void spin(double percent) { + shooterIO.setMotorPercentOutput(percent); + } - // TODO: Add a public method to stop the shooter motor - // public void stop() { ... } + // Add a public method to stop the shooter motor + public void stop() { + shooterIO.setMotorPercentOutput(0.0); + } - // TODO: Add a public method to return the current motor RPM - // public double getCurrentRPM() { ... } + // Add a public method to return the current motor RPM + public double getCurrentRPM() { + return shooterIOInputs.motorRPM; + } } diff --git a/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index 5e92fa6..e6f129d 100644 --- a/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -5,16 +5,18 @@ public class ShooterIOSim implements ShooterIO { // Usually this would be in a Constants file, we have it here for convenience private static final double MAX_RPM = 6000.0; - // TODO: Add a private field `simulatedMotorPercent` to store the simulated motor percentage + // Add a private field `simulatedMotorPercent` to store the simulated motor percentage + private double simulatedMotorPercent = 0.0; @Override public void updateInputs(ShooterIOInputs inputs) { - // TODO: Simulate motorRPM by multiplying the stored simulatedMotorPercent by the max RPM - inputs.motorRPM = 0.0; // Placeholder - replace 0.0 + // Simulate motorRPM by multiplying the stored simulatedMotorPercent by the max RPM + inputs.motorRPM = simulatedMotorPercent * MAX_RPM; // Placeholder - replace 0.0 } @Override public void setMotorPercentOutput(double percent) { - // TODO: Store the given `percent` in your simulated motor percentage field + // Store the given `percent` in your simulated motor percentage field + simulatedMotorPercent = percent; } } diff --git a/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index eec4fbb..a2180fe 100644 --- a/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/tasks/lesson3-shooter-task/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -25,15 +25,16 @@ public ShooterIOTalonFX() { @Override public void updateInputs(ShooterIOInputs inputs) { - // TODO: Read the motor’s velocity (rotations per second) as a double using - // motor.getVelocity().getValueAsDouble() + // Read the motor’s velocity (rotations per second) as a double using + double rps = motor.getVelocity().getValueAsDouble(); - // TODO: Save the RPM to inputs.motorRPM (convert from rotations per second) + // Save the RPM to inputs.motorRPM (convert from rotations per second) + inputs.motorRPM = rps * 60; } @Override public void setMotorPercentOutput(double percent) { - // TODO: Use motor.setControl and the dutyCycleOut variable to set the motor’s percent output - // Tip: Search up documentation for DutyCycleOut methods + // Use motor.setControl and the dutyCycleOut variable to set the motor’s percent output + motor.setControl(dutyCycleOut); } }