From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Sat, 21 Feb 2026 22:39:38 +0000 (-0800)
Subject: a
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=cddfb08bac7bec4b548fd3fcf507c2183ecebe76;p=FRC2026.git

a
---

diff --git a/src/main/java/frc/robot/commands/gpm/ClimbCommand.java b/src/main/java/frc/robot/commands/gpm/ClimbCommand.java
deleted file mode 100644
index 33047e0..0000000
--- a/src/main/java/frc/robot/commands/gpm/ClimbCommand.java
+++ /dev/null
@@ -1,27 +0,0 @@
-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;
-import frc.robot.commands.drive_comm.DriveToPose;
-import frc.robot.constants.FieldConstants;
-import frc.robot.subsystems.Climb.LinearClimb;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-
-public class ClimbCommand extends SequentialCommandGroup{
-
-    public ClimbCommand(LinearClimb climb, Drivetrain drive, PS5Controller controller){
-        addCommands(
-            new ParallelCommandGroup(
-                new InstantCommand(() -> climb.goUp()),
-                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/commands/gpm/ClimbDriveCommand.java b/src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java
new file mode 100644
index 0000000..6819c09
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/ClimbDriveCommand.java
@@ -0,0 +1,23 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.drive_comm.DriveToPose;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.Climb.LinearClimb;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+
+public class ClimbDriveCommand extends SequentialCommandGroup{
+
+    public ClimbDriveCommand(LinearClimb climb, Drivetrain drive){
+        addCommands(
+            new ParallelCommandGroup(
+                new InstantCommand(() -> climb.climbPosition()),
+                new DriveToPose(drive, () -> FieldConstants.getClimbLocation())
+            )
+        );
+    }
+
+
+}
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));
     }