merge in latest climb changes

Author: mwitcpalek

src/main/java/frc/robot/RobotContainer.java

@@ -24,6 +24,8 @@
 import frc.robot.commands.algae.ToggleHasAlgaeCommand;
 import frc.robot.commands.biscuit.HoldBiscuitCommand;
 import frc.robot.commands.biscuit.JogBiscuitCommand;
+import frc.robot.commands.climb.ClimbCommand;
+import frc.robot.commands.climb.ClimbPrepCommand;
 import frc.robot.commands.coral.EnableEjectBeamCommand;
 import frc.robot.commands.coral.OpenLoopCoralCommand;
 import frc.robot.commands.drive.DriveAutonCommand;
@@ -280,7 +282,7 @@ private void configureDriverBindings() {
 
     // climb
     new JoystickButton(driveJoystick, Button.SWA.id)
-        .onTrue(new InstantCommand(() -> robotStateSubsystem.toClimb()));
+        .onTrue(new ClimbCommand(robotStateSubsystem, climbSubsystem));
   }
 
   private void configureOperatorBindings() {
@@ -331,7 +333,9 @@ private void configureOperatorBindings() {
 
     // Prep Climb
     new JoystickButton(xboxController, XboxController.Button.kStart.value)
-        .onTrue(new InstantCommand(() -> robotStateSubsystem.toPrepClimb()));
+        .onTrue(
+            new ClimbPrepCommand(
+                robotStateSubsystem, climbSubsystem, elevatorSubsystem, biscuitSubsystem));
   }
 
   private void configureTestOperatorBindings() {

src/main/java/frc/robot/commands/climb/Climb.java

@@ -1,4 +1,4 @@
-package frc.robot.commands.robotState;
+package frc.robot.commands.climb;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.climb.ClimbSubsystem;
@@ -9,7 +9,7 @@ public class ClimbCommand extends Command {
   ClimbSubsystem climbSubsystem;
 
   public ClimbCommand(RobotStateSubsystem robotState, ClimbSubsystem climbSubsystem) {
-    addRequirements(robotState, climbSubsystem);
+    addRequirements(climbSubsystem);
     this.robotState = robotState;
   }
 

src/main/java/frc/robot/commands/robotState/ClimbCommand.java renamed to src/main/java/frc/robot/commands/climb/ClimbCommand.java

@@ -0,0 +1,25 @@
+package frc.robot.commands.climb;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+import frc.robot.subsystems.climb.ClimbSubsystem;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+
+public class ClimbPrepCommand extends InstantCommand {
+  RobotStateSubsystem robotState;
+
+  public ClimbPrepCommand(
+      RobotStateSubsystem robotState,
+      ClimbSubsystem climbSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      BiscuitSubsystem biscuitSubsystem) {
+    addRequirements(climbSubsystem, elevatorSubsystem, biscuitSubsystem);
+    this.robotState = robotState;
+  }
+
+  @Override
+  public void initialize() {
+    robotState.toPrepClimb();
+  }
+}