Do not control arm for prepare to score when in amp mode

DevilBotz2876 · Apr 2, 2024 · 996649b · 996649b
1 parent 2f81525
commit 996649b
Showing 1 changed file with 18 additions and 12 deletions.
diff --git 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