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 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 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