replaced ClimbDriveCommand

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index e736859890d0cd1a631fa8c9cfa9f49c5998c856..85ed2ed134c88ae84259825f2ede963ce35e017f 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -102,6 +102,7 @@ public class RobotContainer {
       case PrimeJr: // AKA Valence
         spindexer = new Spindexer();
         intake = new Intake();
+        linearClimb = new LinearClimb();
 
       case WaffleHouse: // AKA Betabot
         turret = new Turret();

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 42642621aba2a0cf55f1da1c3f5840f356bf29f5..abc8eb7a22a822f73ea9bd70e34e6a68f7d38aff 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import frc.robot.Robot;
-import frc.robot.commands.gpm.ClimbDriveCommand;
 import frc.robot.commands.gpm.IntakeMovementCommand;
 import frc.robot.commands.gpm.ReverseMotors;
 import frc.robot.commands.gpm.RunSpindexer;
@@ -170,8 +169,15 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                 climb.retract();
             }));
 
-            // Drive to climb position
-            controller.get(PS5Button.TRIANGLE).onTrue(new ClimbDriveCommand(climb, getDrivetrain()));
+            // Go to up position
+            controller.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> {
+                climb.goUp();
+            }));
+
+            // Go to climb position
+            controller.get(PS5Button.TOUCHPAD).onTrue(new InstantCommand(() -> {
+                climb.climbPosition();
+            }));
         }
 
         // Hood