From 387558d6355d6077a8781ad6c1e7a6de9adba66c Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 9 Mar 2019 14:54:54 -0700 Subject: [PATCH] Introduce Failsafe for if the robot position goes belows negative (Sets the negative position to the new 0) --- .../java/org/usfirst/frc4388/robot/subsystems/Arm.java | 7 ++++--- 1 file changed, 4 insertions(+), 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 a2db85c..1abc253 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 @@ -171,7 +171,10 @@ public class Arm extends Subsystem implements ControlLoopable } public void updatePositionPID(double targetPositionInches) { - targetPositionInchesPID = limitPosition(targetPositionInches); + targetPositionInchesPID = limitPosition(targetPositionInches); + if (limitPosition(motor1.getPositionWorld()) == MIN_POSITION_INCHES){ + resetencoder(); + } double startPositionInches = motor1.getPositionWorld(); //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); motor1.set(ControlMode.Position, targetPositionInches); @@ -309,8 +312,6 @@ public class Arm extends Subsystem implements ControlLoopable double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); - - } private void ControlWithJoystickhold(){