Merge pull request #257 from frc5567/LimitSwitch3-20-23

added limit switches for arm and elevator

src/main/java/frc/robot/Arm.java

@@ -23,7 +23,7 @@ public Arm() {
     /**
      * Method used to reset the encoder on the arm to 0.
      */
-    private void zeroEncoders() {
+    public void zeroEncoders() {
         m_arm.getSensorCollection().setIntegratedSensorPosition(0, RobotMap.TIMEOUT_MS);
     }
 

src/main/java/frc/robot/Elevator.java

@@ -20,7 +20,7 @@ public Elevator() {
     /**
      * Resets the encoder postition to zero.
      */
-    private void zeroEncoders() {
+    public void zeroEncoders() {
         m_elevator.setSelectedSensorPosition(0.0, RobotMap.ElevatorConstants.PID_PRIMARY, RobotMap.TIMEOUT_MS);
     }
 

src/main/java/frc/robot/Robot.java

@@ -8,6 +8,7 @@
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -41,6 +42,9 @@ public class Robot extends TimedRobot {
   private int m_delayCounter;
   private int m_outCounter;
 
+  private DigitalInput m_armSwitch;
+  private DigitalInput m_elevatorSwitch;
+
   com.ctre.phoenix.sensors.Pigeon2 m_pigeon;
 
 
@@ -100,6 +104,12 @@ public void robotInit() {
     m_shoulder.init();
     m_claw.init();
 
+    //elevator
+    m_elevatorSwitch = new DigitalInput(8);
+
+    //arm
+    m_armSwitch = new DigitalInput(9);
+
     try {
       m_camera = CameraServer.startAutomaticCapture();
 
@@ -271,6 +281,17 @@ public void teleopPeriodic() {
     if (coDriverInput.m_shoulderPos == ToggleInput.kToggle) {
       m_shoulder.toggleShoulderState();
     }
+
+    if (m_armSwitch.get()) {
+      m_arm.zeroEncoders();
+      //System.out.println("ARM SWITCH zero encoders");
+    }
+
+    if (m_elevatorSwitch.get()) {
+      m_elevator.zeroEncoders();
+      //System.out.println("ELEVATOR SWITCH zero encoders");
+    }
+
   }
 
   /** This function is called once when the robot is disabled. */