From 99a9f76d164fa45c82a787cce0b9a47519db69ea Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 8 Mar 2019 19:29:57 -0700 Subject: [PATCH 1/2] Add limit switches to code Encoder position will zero out whenever the arm hits the bottom limit switch. The Digital Output ids still need to be configured. --- .../java/org/usfirst/frc4388/robot/subsystems/Arm.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index 83c3d05..115d8e7 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -21,6 +21,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -93,6 +94,9 @@ public class Arm extends Subsystem implements ControlLoopable private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later public static final double PID_ERROR_INCHES = 5.0; LimitSwitchSource limitSwitchSource; + DigitalInput forwardLimitSwitch = new DigitalInput(1); + DigitalInput reverseLimitSwitch = new DigitalInput(2); + // Pneumatics private Solenoid speedShift; @@ -270,6 +274,10 @@ public class Arm extends Subsystem implements ControlLoopable } lastControlLoopUpdateTimestamp = currentTimestamp; + if (reverseLimitSwitch.get()){ + motor1.setPosition(0); + } + // Do the update if (armControlMode == ArmControlMode.JOYSTICK_MANUAL) { controlManualWithJoystick(); From 7269697ac93d74d8bef64af76fd2b85496a34e56 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 9 Mar 2019 15:52:24 -0700 Subject: [PATCH 2/2] Update Limit Switch code in Arm.java --- .../main/java/org/usfirst/frc4388/robot/subsystems/Arm.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index 115d8e7..7f67988 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -94,8 +94,6 @@ public class Arm extends Subsystem implements ControlLoopable private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later public static final double PID_ERROR_INCHES = 5.0; LimitSwitchSource limitSwitchSource; - DigitalInput forwardLimitSwitch = new DigitalInput(1); - DigitalInput reverseLimitSwitch = new DigitalInput(2); // Pneumatics private Solenoid speedShift; @@ -274,7 +272,7 @@ public class Arm extends Subsystem implements ControlLoopable } lastControlLoopUpdateTimestamp = currentTimestamp; - if (reverseLimitSwitch.get()){ + if (motor1.getSensorCollection().isRevLimitSwitchClosed()){ motor1.setPosition(0); }