feat: button bindings, wrote manual shooter command

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -14,17 +14,22 @@
 import org.team1540.robot2024.commands.FeedForwardCharacterization;
 import org.team1540.robot2024.commands.SwerveDriveCommand;
 import org.team1540.robot2024.commands.TrampShoot;
+import org.team1540.robot2024.commands.climb.ClimbSequence;
+import org.team1540.robot2024.commands.climb.DeclimbSequence;
 import org.team1540.robot2024.commands.elevator.ElevatorManualCommand;
 import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.commands.indexer.IntakeCommand;
 import org.team1540.robot2024.commands.indexer.StageTrampCommand;
+import org.team1540.robot2024.commands.shooter.ManualShooterCommand;
+import org.team1540.robot2024.commands.shooter.PrepareShooterCommand;
 import org.team1540.robot2024.commands.shooter.ShootSequence;
 import org.team1540.robot2024.commands.shooter.TuneShooterCommand;
 import org.team1540.robot2024.subsystems.drive.*;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
 import org.team1540.robot2024.subsystems.elevator.ElevatorIOSim;
 import org.team1540.robot2024.subsystems.elevator.ElevatorIOTalonFX;
+import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
 import org.team1540.robot2024.subsystems.indexer.Indexer;
 import org.team1540.robot2024.subsystems.indexer.IndexerIO;
 import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
@@ -56,6 +61,7 @@ public class RobotContainer {
     public final Tramp tramp;
     public final Shooter shooter;
     public final Elevator elevator;
+    public final Hooks hook;
     public final Indexer indexer;
     public final AprilTagVision aprilTagVision;
 
@@ -87,6 +93,7 @@ public RobotContainer() {
                 tramp = new Tramp(new TrampIOSparkMax());
                 shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
                 elevator = new Elevator(new ElevatorIOTalonFX());
+                hook = new Hooks();
                 indexer =
                         new Indexer(
                                 new IndexerIOSparkMax()
@@ -111,6 +118,7 @@ public RobotContainer() {
                 tramp = new Tramp(new TrampIOSim());
                 shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
                 elevator = new Elevator(new ElevatorIOSim());
+                hook = new Hooks();
                 aprilTagVision =
                         new AprilTagVision(
                                 new AprilTagVisionIOSim(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE, drivetrain::getPose),
@@ -134,6 +142,7 @@ public RobotContainer() {
                                 new ModuleIO() {});
                 shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
                 elevator = new Elevator(new ElevatorIO() {});
+                hook = new Hooks();
                 aprilTagVision =
                         new AprilTagVision(
                                 new AprilTagVisionIO() {},
@@ -174,27 +183,32 @@ public RobotContainer() {
      * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
      */
     private void configureButtonBindings() {
+        // driver left (x,y) and right (x,y) -> drive
         drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver));
-        elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot));
-        copilot.rightBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.TOP));
-        copilot.leftBumper().onTrue(new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM));
+        // driver y -> zero field orientation, x -> x stop
         driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain));
-        driver.b().onTrue(
+        driver.y().onTrue(
                 Commands.runOnce(
                         () -> drivetrain.setPose(new Pose2d(drivetrain.getPose().getTranslation(), new Rotation2d())),
                         drivetrain
                 ).ignoringDisable(true)
         );
-        driver.rightBumper().onTrue(new IntakeCommand(indexer));
-        driver.leftBumper().onTrue(new TrampShoot(tramp));
-        //TODO: not sure how to do angling/don't think there's a command for this yet?
-
-        copilot.a().onTrue(new ShootSequence(shooter, indexer))
+        // driver left bumper -> shooter align and shoot
+        driver.leftBumper().onTrue(new ShootSequence(shooter, indexer))
                 .onFalse(Commands.runOnce(shooter::stopFlywheels, shooter));
-        copilot.rightBumper().onTrue(new StageTrampCommand(tramp, indexer));
+        //TODO: driver needs angling command (Alvin wrote it)
 
-        // copilot: shooter staging, climbing, shooter override, elevator override, signaling
-        // use tramp stage
+        // copilot left and right trigger -> elevator up and down
+        elevator.setDefaultCommand(new ElevatorManualCommand(elevator, copilot));
+        // copilot left y -> shooter angle
+        shooter.setDefaultCommand(new ManualShooterCommand(shooter, copilot));
+        // copilot left and right bumper -> climb up climb down
+        copilot.leftBumper().onTrue(new ClimbSequence(elevator, hook));
+        copilot.rightBumper().onTrue(new DeclimbSequence(elevator, hook));
+        // copilot a + b -> stage trap + stage shooter
+        copilot.a().onTrue(new StageTrampCommand(tramp, indexer));
+        copilot.b().onTrue(new PrepareShooterCommand(shooter));
+        //TODO: copilot needs signaling? (not sure how this works)
     }
 
     /**

src/main/java/org/team1540/robot2024/commands/shooter/ManualShooterCommand.java

@@ -0,0 +1,30 @@
+package org.team1540.robot2024.commands.shooter;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import org.team1540.robot2024.subsystems.shooter.Shooter;
+
+public class ManualShooterCommand extends Command{
+        private final Shooter shooter;
+        private final CommandXboxController controller;
+
+        public ManualShooterCommand(Shooter shooter, CommandXboxController controller) {
+            this.shooter = shooter;
+            addRequirements(shooter);
+            this.controller = controller;
+        }
+        @Override
+        public void execute() {
+            Rotation2d angleRotation2d = shooter.getPivotPosition();
+            shooter.setPivotPosition(angleRotation2d.plus(Rotation2d.fromRotations(0.01*(controller.getLeftY()))));
+            // controller values range from -1 to 1
+            // pivot position will be within 90 degrees
+        }
+
+        @Override
+        public boolean isFinished() {
+            return shooter.isPivotAtSetpoint();
+        }
+    }
+

src/main/java/org/team1540/robot2024/commands/shooter/PrepareShooterCommand.java

@@ -4,7 +4,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import org.team1540.robot2024.subsystems.shooter.Shooter;
 
-class PrepareShooterCommand extends Command {
+public class PrepareShooterCommand extends Command {
     private final Shooter shooter;
 
     public PrepareShooterCommand(Shooter shooter) {

src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java

@@ -4,9 +4,13 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-//TODO: Get rid of this class and make an actual one its just a temporary thingie for David :D
+//TODO: Get rid of this class and make an actual one its just a temporary thing for David :D
 public class Hooks extends SubsystemBase {
 
+    public Hooks(){
+        //something
+    }
+
     //TODO: Whoever does this it should retract or deploy the hooks depending on if they are already deployed or retracted
     public void deployHooks() {
 
@@ -24,7 +28,7 @@ public Command deployHooksCommand() {
     }
 
     /**
-     * Factory method for undeploying hooks
+     * Factory method for un-deploying hooks
      */
     public Command undeployHooksCommand() {
         return new InstantCommand(this::undeployHooks, this);