From cddfb08bac7bec4b548fd3fcf507c2183ecebe76 Mon Sep 17 00:00:00 2001
From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Sat, 21 Feb 2026 14:39:38 -0800
Subject: [PATCH] a

---
 ...imbCommand.java => ClimbDriveCommand.java} | 12 +++------
 .../controls/PS5ControllerDriverConfig.java   | 26 +++++++++++++++++--
 .../robot/subsystems/Climb/LinearClimb.java   |  8 +++---
 3 files changed, 32 insertions(+), 14 deletions(-)
 rename src/main/java/frc/robot/commands/gpm/{ClimbCommand.java => ClimbDriveCommand.java} (54%)

diff --git a/src/main/java/frc/robot/commands/gpm/ClimbCommand.java b/src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java
similarity index 54%
rename from src/main/java/frc/robot/commands/gpm/ClimbCommand.java
rename to src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java
index 33047e0..6819c09 100644
--- a/src/main/java/frc/robot/commands/gpm/ClimbCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java
@@ -1,7 +1,5 @@
 package frc.robot.commands.gpm;
 
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.PS5Controller;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -10,16 +8,14 @@ import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 
-public class ClimbCommand extends SequentialCommandGroup{
+public class ClimbDriveCommand extends SequentialCommandGroup{
 
-    public ClimbCommand(LinearClimb climb, Drivetrain drive, PS5Controller controller){
+    public ClimbDriveCommand(LinearClimb climb, Drivetrain drive){
         addCommands(
             new ParallelCommandGroup(
-                new InstantCommand(() -> climb.goUp()),
+                new InstantCommand(() -> climb.climbPosition()),
                 new DriveToPose(drive, () -> FieldConstants.getClimbLocation())
-            ),
-            // TODO: Make it stop rumbling after like a second
-            new InstantCommand(() -> controller.setRumble(GenericHID.RumbleType.kBothRumble, 1.0)) 
+            )
         );
     }
 
diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index e916934..61b6d17 100644
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -9,8 +9,11 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Robot;
 import frc.robot.commands.gpm.AutoShootCommand;
+import frc.robot.commands.gpm.ClimbDriveCommand;
 import frc.robot.commands.gpm.ReverseMotors;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.Climb.LinearClimb;
@@ -145,9 +148,28 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                 climb.stopCalibrating();
             }));
 
-            // Climb climb
+            // Climb retract
             driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()-> {
-                climb.goDown();
+                climb.retract();
+            }));
+
+            // Drive to climb position and rumble
+            driver.get(PS5Button.TRIANGLE).onTrue(new SequentialCommandGroup(
+                new ClimbDriveCommand(climb, getDrivetrain()),
+                new InstantCommand(()-> this.startRumble()),
+                new WaitCommand(1),
+                new InstantCommand(()-> this.endRumble())
+            )
+            );
+        }
+
+        // Hood
+        if (hood != null) {
+            // Calibration
+            driver.get(PS5Button.PS).onTrue(new InstantCommand(()->{
+                hood.calibrate();
+            })).onFalse(new InstantCommand(()->{
+                hood.stopCalibrating();
             }));
         }
     }
diff --git a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
index 2b14eba..784aaf6 100644
--- a/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
+++ b/src/main/java/frc/robot/subsystems/Climb/LinearClimb.java
@@ -56,8 +56,8 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
         setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT);
 
         SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp()));
-        SmartDashboard.putData("Go Down", new InstantCommand(() -> goDown()));
-        SmartDashboard.putData("Climb", new InstantCommand(() -> climb()));
+        SmartDashboard.putData("Go Down", new InstantCommand(() -> retract()));
+        SmartDashboard.putData("Climb", new InstantCommand(() -> climbPosition()));
 
         motor.setPosition(0);
 
@@ -116,7 +116,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
     /**
      * goes to the down position
      */
-    public void goDown() {
+    public void retract() {
         MAX_POWER = 0.2;
         setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION));
     }
@@ -124,7 +124,7 @@ public class LinearClimb extends SubsystemBase implements LinearClimbIO{
     /**
      * goes to the climb position
      */
-    public void climb() {
+    public void climbPosition() {
         MAX_POWER = 0.8;
         setSetpoint(metersToRotations(ClimbConstants.CLIMB_POSITION));
     }
-- 
2.39.5