diff --git a/src/main/java/frc/robot/Arm.java b/src/main/java/frc/robot/Arm.java index b1149bc..b99122a 100644 --- a/src/main/java/frc/robot/Arm.java +++ b/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); } diff --git a/src/main/java/frc/robot/Elevator.java b/src/main/java/frc/robot/Elevator.java index 3393e66..f3fd078 100644 --- a/src/main/java/frc/robot/Elevator.java +++ b/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); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4fde9ee..d2255a7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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. */