thing and controls for calibrate yay

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 8e0cce8df809fcb1c46229058f8473b2d7c2c100..4d743216c2118640e3c3c487fa608a527034269e 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -99,7 +99,7 @@ public class RobotContainer {
 
       case Vertigo: // AKA "French Toast"
         drive = new Drivetrain(vision, new GyroIOPigeon2());
-        driver = new PS5ControllerDriverConfig(drive);
+        driver = new PS5ControllerDriverConfig(drive, linearClimb);
         operator = new Operator(drive);
         // added indexer here for now
 

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 6ef2c090bbbc04de7d5ad73979b88e3ee5b650f5..ce9e7270261cf364934614c90915652934dd0887 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.Robot;
 import frc.robot.constants.Constants;
+import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import lib.controllers.PS5Controller;
 import lib.controllers.PS5Controller.PS5Axis;
@@ -20,9 +21,11 @@ import lib.controllers.PS5Controller.PS5Button;
 public class PS5ControllerDriverConfig extends BaseDriverConfig {
     private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY);
     private final BooleanSupplier slowModeSupplier = ()->false;
+    private LinearClimb climb;
 
-    public PS5ControllerDriverConfig(Drivetrain drive) {
+    public PS5ControllerDriverConfig(Drivetrain drive, LinearClimb climb) {
         super(drive);
+        this.climb = climb;
     }
 
     public void configureControls() {
@@ -44,6 +47,14 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             getDrivetrain()::alignWheels,
             interrupted->getDrivetrain().setStateDeadband(true),
             ()->false, getDrivetrain()).withTimeout(2));
+        
+        if (climb != null) {
+            driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {
+                climb.hardstopCalibration();
+            })).onFalse(new InstantCommand(() -> {
+                climb.stopCalibrating();
+            }));
+        }
     }
     
     @Override