diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 669e06a..e795c5d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -71,6 +71,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.commands.BasicCommandChooser; import frc4388.robot.commands.ComplexCommandChooser; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; import frc4388.robot.commands.ButtonBoxCommands.RunTurretOrClimberAuto; @@ -170,6 +171,7 @@ public class RobotContainer { // climber mode switching // private enum ClimberMode { MANUAL, AUTONOMOUS }; private ControlMode currentClimberMode = ControlMode.MANUAL; + private boolean command1 = true; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -361,40 +363,17 @@ public class RobotContainer { // toggle manual mode and autonomous mode based on the current control mode new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + .whenPressed(new InstantCommand(() -> this.command1 = !this.command1)) + .whenPressed(new BasicCommandChooser( new RunCommand(() -> System.out.println("COMMAND 1")), + new RunCommand(() -> System.out.println("COMMAND 2")), + () -> this.command1 == true, + () -> this.command1 == false)) + .whenReleased(new InstantCommand(() -> this.command1 = !this.command1)); - // .whenPressed(new InstantCommand(() -> { - // if (this.currentControlMode.equals(SubsystemMode.CLIMBER)) { m_robotTurret.gotoZero(); } - // }, m_robotTurret)) - - // .whenPressed(new InstantCommand(() -> { - // if (this.currentControlMode.equals(SubsystemMode.SHOOTER)) { this.currentTurretMode = ControlMode.MANUAL; } - // if (this.currentControlMode.equals(SubsystemMode.CLIMBER)) { this.currentClimberMode = ControlMode.AUTONOMOUS; } - // })) - - // * custom Command inside InstantCommand - // .whenPressed(new InstantCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}); } - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry); } - // }) - // .until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) - - // * external RunTurretOrClimberAutos command, which runs either one (conditionally) based on currentControlMode - // .whenPressed(new RunTurretOrClimberAuto(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry, m_robotClimber, m_robotClaws)) - - // * CommandChooser with BooleanSuppliers - .whenPressed(new ComplexCommandChooser(new HashMap() {{ - put(new InstantCommand(() -> System.out.println("true")), () -> true); - put(new InstantCommand(() -> System.out.println("false")), () -> false); - }})); - // .whenPressed(new CommandChooser(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}), - // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry), - // () -> this.currentControlMode.equals(SubsystemMode.CLIMBER), - // () -> this.currentControlMode.equals(SubsystemMode.SHOOTER))) - - // .whenReleased(new InstantCommand(() -> { - // if (this.currentControlMode.equals(SubsystemMode.SHOOTER)) { this.currentTurretMode = ControlMode.AUTONOMOUS; } - // if (this.currentControlMode.equals(SubsystemMode.CLIMBER)) { this.currentClimberMode = ControlMode.MANUAL; } - // })); + // .whenPressed(new ComplexCommandChooser(new HashMap() {{ + // put(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}), () -> currentControlMode.equals(SubsystemMode.CLIMBER)); + // put(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry), () -> currentControlMode.equals(SubsystemMode.SHOOTER)); + // }})); new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))