Add climber sequencing to driver controller (#204)

* add trap and simple climb sequences * .. * Add name for teleop drive * Check style * Fix typo * Finish climb sequence --------- Co-authored-by: Jonah <[email protected]>
Mechanical-Advantage · Mar 14, 2024 · 72cb73b · 72cb73b
1 parent 292b9f8
commit 72cb73b
Show file tree

Hide file tree

Showing 3 changed files with 123 additions and 265 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java
@@ -28,6 +28,7 @@
 import java.util.HashMap;
 import java.util.Map;
 import java.util.function.BiConsumer;
+import java.util.function.DoubleSupplier;
 import org.littletonrobotics.frc2024.Constants.Mode;
 import org.littletonrobotics.frc2024.subsystems.leds.Leds;
 import org.littletonrobotics.frc2024.util.Alert;
@@ -83,12 +84,12 @@ public class Robot extends LoggedRobot {
   private final Alert sameBatteryAlert =
       new Alert("The battery has not been changed since the last match.", AlertType.WARNING);
 
-  public static Trigger createTeleopTimeTrigger(double teleElapsedTime) {
+  public static Trigger createTeleopTimeTrigger(DoubleSupplier teleElapsedTime) {
     return new Trigger(
         () ->
             DriverStation.isFMSAttached()
                 && DriverStation.isTeleopEnabled()
-                && Robot.teleElapsedTime > teleElapsedTime);
+                && Robot.teleElapsedTime > teleElapsedTime.getAsDouble());
   }
 
   /**

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
@@ -19,7 +19,6 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -122,7 +121,6 @@ public class RobotContainer {
       new LoggedDashboardNumber("Endgame Alert #2", 15.0);
 
   private boolean podiumShotMode = false;
-  private boolean trapScoreMode = true;
   private boolean armCoastOverride = false;
 
   // Dashboard inputs
@@ -415,7 +413,6 @@ private void configureAutos() {
    */
   private void configureButtonBindings() {
     // ------------- Driver Controls -------------
-    // Drive command
     drive.setDefaultCommand(
         drive
             .run(
@@ -632,17 +629,22 @@ private void configureButtonBindings() {
         .whileTrue(rollers.setGoalCommand(Rollers.Goal.AMP_SCORE).onlyWhile(driver.rightTrigger()));
 
     // ------------- Climbing Controls -------------
+    Command trapSequence =
+        ClimbingCommands.trapSequence(
+            drive, superstructure, rollers, driver.povUp(), driver.povDown());
+    Command simpleSequence =
+        ClimbingCommands.simpleSequence(superstructure, driver.povUp(), driver.povDown());
     driver
-        .x()
-        .and(
-            () ->
-                superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_CLIMB
-                    && superstructure.getCurrentGoal() != Superstructure.Goal.CANCEL_PREPARE_CLIMB)
-        .toggleOnTrue(
-            Commands.either(
-                superstructure.setGoalCommand(Superstructure.Goal.PREPARE_PREPARE_TRAP_CLIMB),
-                Commands.none(), // No function yet
-                () -> trapScoreMode));
+        .start()
+        .and(driver.back())
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  simpleSequence.cancel();
+                  trapSequence.cancel();
+                }));
+    driver.x().doublePress().onTrue(trapSequence);
+    driver.y().doublePress().onTrue(simpleSequence);
 
     // ------------- Operator Controls -------------
     // Adjust shot compensation
@@ -669,34 +671,6 @@ private void configureButtonBindings() {
         .a()
         .onTrue(Commands.runOnce(() -> podiumShotMode = false)); // set preset mode to subwoofer
 
-    // Climber controls
-    operator.rightStick().onTrue(Commands.runOnce(() -> trapScoreMode = !trapScoreMode));
-    operator
-        .leftBumper()
-        .doublePress()
-        .and(() -> trapScoreMode)
-        .toggleOnTrue(
-            ClimbingCommands.climbNTrapSequence(
-                    drive,
-                    superstructure,
-                    rollers,
-                    () -> -driver.getLeftY(),
-                    () -> -driver.getLeftX(),
-                    () -> -driver.getRightX(),
-                    operator.rightBumper().doublePress(),
-                    operator.start().doublePress().or(operator.back().doublePress()),
-                    autoDriveDisable)
-                .withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
-    operator
-        .leftBumper()
-        .doublePress()
-        .and(() -> !trapScoreMode)
-        .toggleOnTrue(
-            ClimbingCommands.simpleClimbSequence(
-                    superstructure, operator.rightBumper().doublePress())
-                .withInterruptBehavior(InterruptionBehavior.kCancelIncoming));
-    operator.leftStick().onTrue(superstructure.setGoalCommand(Superstructure.Goal.RESET));
-
     // Request amp
     operator
         .x()
@@ -769,7 +743,6 @@ public void updateDashboardOutputs() {
         "Shot Compensation Degrees",
         String.format("%.1f", robotState.getShotCompensationDegrees()));
     SmartDashboard.putBoolean("Podium Preset", podiumShotMode);
-    SmartDashboard.putBoolean("Trap Score Mode", trapScoreMode);
     SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime());
   }