diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9cddcbf..4793fd8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -214,26 +214,25 @@ public class RobotContainer { // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> { - // 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)); + m_robotTurret.setDefaultCommand( + new RunCommand(() -> { + 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)); - // m_robotHood.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } - // }, m_robotHood)); + m_robotHood.setDefaultCommand( + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } + }, m_robotHood)); m_robotClimber.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), - getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //} - // }, m_robotClimber)); + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), getOperatorController().getRightY()); } + }, m_robotClimber)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -355,6 +354,7 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); // .whenReleased(EnableClimber())); + // toggle manual mode and autonomous mode based on the current control mode new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) .whenPressed(new InstantCommand(() -> {