Safety added for climber and leveler

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)));

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;
   }
 }

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);
+    }
   }
 }