diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4d8bc66..fa1ed02 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -133,6 +133,11 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + // safety for climber and leveler + new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) + .whileHeld(new InstantCommand(() -> m_robotClimber.setSafetyPressed(), m_robotClimber)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setSafetyNotPressed(), m_robotClimber)); + /* Storage Neo PID Test */ new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whileHeld(new RunCommand(() -> m_robotStorage.runStoragePositionPID(0.5, 0.2, 0.0, 0.0, 0.0, 0.0, 1, -1))); diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index e878d85..f6c9339 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -19,6 +19,9 @@ import frc4388.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless); CANDigitalInput m_climberForwardLimit, m_climberReverseLimit; + + public boolean climberSafety = false; + /** * Creates a new Climber. */ @@ -44,6 +47,23 @@ public class Climber extends SubsystemBase { * @param input the voltage to run motor at */ public void runClimber(double input) { - m_climberMotor.set(input); + if(climberSafety){ + m_climberMotor.set(input); + } + else{ + m_climberMotor.set(0); + } + } + + /* Safety Button for Climber */ + public void setSafetyPressed() + { + climberSafety = true; + } + + /* Safety Button for Climber set back to false */ + public void setSafetyNotPressed() + { + climberSafety = false; } } diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java index aa93f13..0db0562 100644 --- a/src/main/java/frc4388/robot/subsystems/Leveler.java +++ b/src/main/java/frc4388/robot/subsystems/Leveler.java @@ -17,10 +17,13 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LevelerConstants; +import frc4388.robot.subsystems.*; public class Leveler extends SubsystemBase { CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless); + private final Climber m_robotClimber = new Climber(); + /** * Creates a new Leveler. */ @@ -41,6 +44,11 @@ public class Leveler extends SubsystemBase { * @param input the percent output to run motor at */ public void runLeveler(double input) { - m_levelerMotor.set(input); + if(m_robotClimber.climberSafety){ + m_levelerMotor.set(input); + } + else{ + m_levelerMotor.set(0); + } } }