diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..97fb8c4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,13 +6,37 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Flywheel; public class RobotContainer { + // Create an instance of the Arm subsystem + private final Arm arm = new Arm(); + + // Create an instance of the Flywheel subsystem + private final Flywheel flywheel = new Flywheel(); + + // Create a joystick to run subsystems off of + private final CommandXboxController joystick = new CommandXboxController(0); + public RobotContainer() { configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + // When left trigger is pressed the arm will run at a fast speed, and when not pressed it will + // stop + joystick.leftTrigger().onTrue(arm.runFast()).onFalse(arm.stopCommand()); + + // When right trigger is pressed the flywheel will run at a fast speed, and when not pressed it + // will spin slow + joystick.rightTrigger().onTrue(flywheel.runFast()).onFalse(flywheel.runSlow()); + + // When A button is pressed the flywheel will run at a fast speed, and when not pressed it will + // stop + joystick.a().onTrue(flywheel.runFast()).onFalse(flywheel.stopCommand()); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index a050dab..0f19ef2 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -12,6 +12,7 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { @@ -58,6 +59,35 @@ public void setVoltage(double voltage) { leader.setControl(voltageOut.withOutput(voltage)); } + /** + * Command to run the arm at a slow speed. + * + * @return The command to run the arm slowly. + */ + public Command runSlow() { + // Command to run the arm at a slow speed and stop it afterward + return runOnce(() -> setVoltage(3)); + } + + /** + * Command to run the arm at a fast speed. + * + * @return The command to run the arm quickly. + */ + public Command runFast() { + // Command to run the arm at a fast speed and stop it afterward + return runOnce(() -> setVoltage(6)); + } + + /** + * Command to stop the arm. + * + * @return The command to stop the arm. + */ + public Command stopCommand() { + return runOnce(() -> stop()); + } + // Stop the arm motor public void stop() { leader.stopMotor(); diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index c15b556..db7cbcd 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Flywheel extends SubsystemBase { @@ -56,6 +57,35 @@ public void setVoltage(double voltage) { leader.setControl(voltageOut.withOutput(voltage)); } + /** + * Command to run the flywheel at a slow speed. + * + * @return The command to run the flywheel slowly. + */ + public Command runSlow() { + // Command to run the flywheel at a slow speed + return runOnce(() -> setVoltage(3)); + } + + /** + * Command to run the flywheel at a fast speed. + * + * @return The command to run the flywheel fast. + */ + public Command runFast() { + // Command to run the flywheel at a fast speed + return runOnce(() -> setVoltage(6)); + } + + /** + * Command to stop the flywheel. + * + * @return The command to stop the flywheel. + */ + public Command stopCommand() { + return runOnce(() -> stop()); + } + // Stop the flywheel motors public void stop() { leader.stopMotor();