From a12bd05df118a6145d271f49ec1a93826a0f28a6 Mon Sep 17 00:00:00 2001
From: Nilesh Agarwalla <nilesh@alum.mit.edu>
Date: Mon, 1 Apr 2024 20:36:04 -0400
Subject: [PATCH] Split rightBumper to 4 separate triggers

---
 .../frc/robot/controls/DriverControls.java    | 64 ++++++++++---------
 1 file changed, 35 insertions(+), 29 deletions(-)

diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java
index 2768c629..4378cd02 100644
--- a/src/main/java/frc/robot/controls/DriverControls.java
+++ b/src/main/java/frc/robot/controls/DriverControls.java
@@ -293,38 +293,44 @@ public static void setupMainControls(CommandXboxController mainController) {
         .onTrue(
             new EjectPiece(RobotConfig.intake, RobotConfig.arm, RobotConfig.shooter)); // Eject Note
 
+    mainController.rightBumper().onTrue(RobotConfig.intake.getTurnOffCommand());
+
     mainController
         .rightBumper()
         .onTrue(
-            new ParallelCommandGroup(
-                RobotConfig.intake.getTurnOffCommand(),
-                new SelectCommand<>(
-                    Map.ofEntries(
-                        Map.entry(
-                            true,
-                            AutoBuilder.pathfindToPoseFlipped(
-                                new Pose2d(1.8, 7.75, Rotation2d.fromDegrees(-90)),
-                                new PathConstraints(4.0, 3.0, 2 * Math.PI, 3 * Math.PI))),
-                        Map.entry(
-                            false,
-                            new DriveToYaw(
-                                    RobotConfig.drive,
-                                    () -> DevilBotState.getVisionRobotYawToTarget())
-                                .withTimeout(DriveConstants.pidTimeoutInSeconds))),
-                    () -> DevilBotState.isAmpMode()),
-                new SetShooterVelocity(
-                        RobotConfig.shooter, () -> DevilBotState.getShooterVelocity())
-                    .withTimeout(ShooterConstants.pidTimeoutInSeconds), // turn on shooter
-                /* TODO: Use ArmToPositionTP instead of setting arm angle directly */
-                new InstantCommand(
-                        () -> {
-                          Optional<Double> armAngle = DevilBotState.getArmAngleToTarget();
-                          if (armAngle.isPresent()) {
-                            RobotConfig.arm.setAngle((armAngle.get()));
-                          }
-                        },
-                        RobotConfig.arm)
-                    .onlyIf(() -> !DevilBotState.isAmpMode()))); // Aim
+            new SelectCommand<>(
+                Map.ofEntries(
+                    Map.entry(
+                        true,
+                        AutoBuilder.pathfindToPoseFlipped(
+                            new Pose2d(1.8, 7.75, Rotation2d.fromDegrees(-90)),
+                            new PathConstraints(4.0, 3.0, 2 * Math.PI, 3 * Math.PI))),
+                    Map.entry(
+                        false,
+                        new DriveToYaw(
+                                RobotConfig.drive, () -> DevilBotState.getVisionRobotYawToTarget())
+                            .withTimeout(DriveConstants.pidTimeoutInSeconds))),
+                () -> DevilBotState.isAmpMode()));
+
+    mainController
+        .rightBumper()
+        .onTrue(
+            new SetShooterVelocity(RobotConfig.shooter, () -> DevilBotState.getShooterVelocity())
+                .withTimeout(ShooterConstants.pidTimeoutInSeconds) // turn on shooter
+            );
+
+    mainController
+        .rightBumper()
+        .onTrue(
+            new InstantCommand(
+                    () -> {
+                      Optional<Double> armAngle = DevilBotState.getArmAngleToTarget();
+                      if (armAngle.isPresent()) {
+                        RobotConfig.arm.setAngle((armAngle.get()));
+                      }
+                    },
+                    RobotConfig.arm)
+                .onlyIf(() -> !DevilBotState.isAmpMode()));
 
     mainController
         .rightTrigger()