Merge pull request #35 from Team4388/Climber-and-Leveler-safety-button

Safety added for climber and leveler

src/main/java/frc4388/robot/Constants.java

@@ -72,9 +72,9 @@ public final class Constants {
   
     public static final class ShooterConstants {
         /* Motor IDs */
-        public static final int SHOOTER_FALCON_ID = 8;
+        public static final int SHOOTER_FALCON_ID = -1;
-        public static final int SHOOTER_ANGLE_ADJUST_ID = 9;
+        public static final int SHOOTER_ANGLE_ADJUST_ID = -1;
-        public static final int SHOOTER_ROTATE_ID = 10;
+        public static final int SHOOTER_ROTATE_ID = -1;
 
         /* PID Constants Shooter */
         public static final int SHOOTER_SLOT_IDX = 0;
@@ -89,7 +89,7 @@ public final class Constants {
     }
     
     public static final class ClimberConstants {
-        public static final int CLIMBER_SPARK_ID = -1;
+        public static final int CLIMBER_SPARK_ID = 10;
     }
   
     public static final class LevelerConstants {
@@ -97,7 +97,7 @@ public final class Constants {
     }
   
     public static final class StorageConstants {
-        public static final int STORAGE_CAN_ID = 10;
+        public static final int STORAGE_CAN_ID = -1;
 
         /* Ball Indexes */
         public static final int BEAM_SENSOR_DIO_0 = 0;

src/main/java/frc4388/robot/RobotContainer.java

@@ -147,6 +147,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.BACK_BUTTON)
+            .whenPressed(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);
+    }
   }
 }