diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java index 4a709c7..e6a982e 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java @@ -21,7 +21,8 @@ public class ResetLower extends CommandBase { } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { - return mainArm.getBottomPressed(); + // Stop when either limit switch is hit + return mainArm.getBottomPressed() || mainArm.getTopPressed(); } // Called once after isFinished returns true protected void end() { diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetUpper.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetUpper.java index 3c15919..d52e1da 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetUpper.java +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetUpper.java @@ -21,7 +21,8 @@ public class ResetUpper extends CommandBase { } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { - return mainArm.getTopPressed(); + // Stop when either limit switch is hit + return mainArm.getBottomPressed() || mainArm.getTopPressed(); } // Called once after isFinished returns true protected void end() { diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java index b001895..41d3fbd 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java @@ -27,7 +27,8 @@ public class SetArmPosition extends CommandBase { } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { - return false; + // Stop when either limit switch is hit + return mainArm.getBottomPressed() || mainArm.getTopPressed(); } // Called once after isFinished returns true protected void end() { diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 9674b70..7c9c398 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -31,12 +31,7 @@ public class MainArm extends PIDSubsystem { return getDegrees(); } protected void usePIDOutput(double output) { - // Only run if neither limit switch is hit - if (!calibrate()) { - moveArm(output); - } else { - moveArm(0); - } + moveArm(output); } public double getRaw() { return pot.getAverageVoltage();