Stow shooter when climbing, add shooter stow control

frc868 · Apr 13, 2024 · c8ed193 · c8ed193
1 parent cde42ac
commit c8ed193
Showing 1 changed file with 9 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.Constants.Climber.ClimberPosition;
 import frc.robot.Constants.Intake.IntakePosition;
 import frc.robot.Constants.ShooterTilt.ShooterTiltPosition;
 import frc.robot.subsystems.Climber;
@@ -46,15 +47,18 @@ public static void configureDriverControls(int port, Drivetrain drivetrain, Inta
                 .whileTrue(intake.intakeNoteCommand()
                         .alongWith(shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.INTAKE).asProxy()))
                 .onFalse(intake.moveToPositionCommand(() -> IntakePosition.STOW));
-
         joystick.centerTopHatButton().whileTrue(
                 Commands.parallel(
                         intake.intakeFromSourceCommand(),
                         leds.requestStateCommand(LEDState.FLASHING_AQUA)))
                 .onFalse(intake.moveToPositionCommand(() -> IntakePosition.STOW));
 
-        joystick.centerTopHatUp().whileTrue(climber.moveUpCommand());
-        joystick.centerTopHatDown().whileTrue(climber.moveDownCommand());
+        joystick.centerTopHatUp().whileTrue(
+                climber.moveUpCommand()
+                        .deadlineWith(shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.PODIUM)));
+        joystick.centerTopHatDown().whileTrue(
+                climber.moveDownCommand()
+                        .deadlineWith(shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.PODIUM)));
 
         joystick.redButton().whileTrue(RobotCommands.ampPrepIntakeCommand(intake, shooterTilt))
                 .onFalse(intake.moveToPositionCommand(() -> IntakePosition.AMP));
@@ -135,7 +139,8 @@ public static void configureOverridesControls(int port, Drivetrain drivetrain, I
                 GlobalStates.MECH_LIMITS_DISABLED.disableCommand()
                         .andThen(GlobalStates.INTER_SUBSYSTEM_SAFETIES_DISABLED.disableCommand()));
 
-        controller.a().onTrue(shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.SUBWOOFER));
+        controller.a().onTrue(shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.SUBWOOFER).asProxy()
+                .andThen(climber.moveToPositionCommand(() -> ClimberPosition.STOW).asProxy()));
     }
 
     public static void configureTestingControls(int port, Drivetrain drivetrain, Intake intake, Shooter shooter,