From c434b616ca4fa513310bb2ffd2ae26a196a783ff Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 17:39:06 -0600 Subject: [PATCH] turret manual switching KINDA --- .../java/frc4388/robot/RobotContainer.java | 21 +++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 10acaaf..decf65e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -152,10 +152,15 @@ public class RobotContainer { public static boolean softLimits = true; - // mode switching + // control mode switching private enum ControlMode { SHOOTER, CLIMBER }; private ControlMode currentControlMode = ControlMode.SHOOTER; + // turret mode switching + private enum TurretMode { MANUAL, AUTONOMOUS }; + private TurretMode currentTurretMode = TurretMode.MANUAL; + + // climber mode switching private enum ClimberMode { MANUAL, AUTONOMOUS }; private ClimberMode currentClimberMode = ClimberMode.MANUAL; @@ -209,7 +214,9 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + } if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } }, m_robotTurret)); @@ -340,11 +347,17 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - .whenPressed(new InstantCommand(() -> this.currentClimberMode = ClimberMode.AUTONOMOUS)) - // TODO: actual climber autonomous command goes here (next line) + .whenPressed(new InstantCommand(() -> { + this.currentClimberMode = ClimberMode.AUTONOMOUS; + })) .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) .whenReleased(new InstantCommand(() -> this.currentClimberMode = ClimberMode.MANUAL)); + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + .whenPressed(new InstantCommand(() -> this.currentTurretMode = TurretMode.AUTONOMOUS)) + .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL))) + .whenReleased(new InstantCommand(() -> this.currentTurretMode = TurretMode.MANUAL)); + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));