From 996649be6ce8a5c1dadd9c9e535901737382b16e Mon Sep 17 00:00:00 2001
From: Nilesh Agarwalla <nilesh@alum.mit.edu>
Date: Mon, 1 Apr 2024 20:06:38 -0400
Subject: [PATCH] Do not control arm for prepare to score when in amp mode

---
 .../frc/robot/controls/DriverControls.java    | 30 +++++++++++--------
 1 file changed, 18 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java
index ab7f0f00..c00ce31e 100644
--- a/src/main/java/frc/robot/controls/DriverControls.java
+++ b/src/main/java/frc/robot/controls/DriverControls.java
@@ -316,18 +316,24 @@ public static void setupMainControls(CommandXboxController mainController) {
                         RobotConfig.shooter, () -> DevilBotState.getShooterVelocity())
                     .withTimeout(ShooterConstants.pidTimeoutInSeconds), // turn on shooter
                 /* TODO: Use ArmToPositionTP instead of setting arm angle directly */
-                new InstantCommand(
-                    () -> {
-                      if (DevilBotState.isAmpMode()) {
-                        // RobotConfig.arm.setAngle(ArmConstants.ampScoreAngleInDegrees);
-                      } else {
-                        Optional<Double> armAngle = DevilBotState.getArmAngleToTarget();
-                        if (armAngle.isPresent()) {
-                          RobotConfig.arm.setAngle((armAngle.get()));
-                        }
-                      }
-                    },
-                    RobotConfig.arm) // adjust arm angle based on vision's distance from target
+                new SelectCommand<>(
+                    Map.ofEntries(
+                        // Map.entry(true, new InstantCommand(
+                        // ()->RobotConfig.arm.setAngle(ArmConstants.ampScoreAngleInDegrees),
+                        // RobotConfig.arm)),
+                        Map.entry(
+                            false,
+                            new InstantCommand(
+                                () -> {
+                                  Optional<Double> armAngle = DevilBotState.getArmAngleToTarget();
+                                  if (armAngle.isPresent()) {
+                                    RobotConfig.arm.setAngle((armAngle.get()));
+                                  }
+                                },
+                                RobotConfig.arm))),
+                    () ->
+                        DevilBotState
+                            .isAmpMode()) // adjust arm angle based on vision's distance from target
                 )); // Aim
 
     mainController