Added stuff to prepClimb, but IDK if it's right or not

Author: KaydenLee456

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

@@ -178,7 +178,7 @@ public RobotContainer() {
     configureTelemetry();
     configureDriverBindings();
     configureOperatorBindings();
-    // configureTestOperatorBindings();
+    configureTestOperatorBindings();
     configurePitDashboard();
   }
 

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

@@ -1,20 +1,28 @@
 package frc.robot.commands.climb;
 
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
 import frc.robot.subsystems.climb.ClimbSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
 
-public class PrepClimb {
+public class PrepClimb extends Command {
 
+  RobotStateSubsystem robotStateSubsystem;
   ClimbSubsystem climbSubsystem;
-  /*
+
+  public PrepClimb(ClimbSubsystem climbSubsystem, RobotStateSubsystem robotStateSubsystem) {
+    this.climbSubsystem = climbSubsystem;
+    this.robotStateSubsystem = robotStateSubsystem;
+  }
+  
   @Override
   public void initialize() {
-
+      robotStateSubsystem.toClimb();
   }
 
   @Override
-  public void isFinished() {
-
+  public boolean isFinished() {
+    return (robotStateSubsystem.getState() != RobotStates.PREP_CLIMB);
   }
-  */
 
 }

src/main/java/frc/robot/constants/ClimbConstants.java

@@ -42,7 +42,7 @@ public class ClimbConstants {
   public static final double kRatchetDisengagedPos = 1.0;
 
   // Climb positions
-  // public static final Angle kClimbCagePos = Rotations.of(); // will test
+  // public static final Angle kClimbCagePos = Rotations.of();
   public static final Double kClimbRatchedEngage = -0.12;
   public static final double kFullyClimbed = kArmMaxFwd;
   public static final double kClimbOpenLoopSpeed = 4.0;