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 (file)
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 (file)
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 e9169348f592575b034c0e699ac39095aa3a6081..61b6d171bd11d32d61e58849c26c6d1e02de7d34 100644 (file)
--- 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 2b14eba9881d42d6b68215a0b03099c031aaa246..784aaf69637163190ca8984fa40cc3bd5f360eed 100644 (file)
--- 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));
     }