change command to moving to level instead of target angle and simplify isFinished

diff --git a/src/org/usfirst/frc/team3501/robot/commands/SetHandToLevel.java b/src/org/usfirst/frc/team3501/robot/commands/SetHandToLevel.java
index 11aae28a49e83ed03eae290e31a3d85d19c15e15..3d7fb7486bfce3984080223e9d5370f892b36f2b 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/commands/SetHandToLevel.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/SetHandToLevel.java
@@ -9,7 +9,6 @@ public class SetHandToLevel extends Command {
   private double speed;
   private double targetPosition;
   private double currentPosition;
-  private boolean isDecreasing = false;
 
   public SetHandToLevel(double speed, int level) {
     requires(Robot.defenseArm);
@@ -24,10 +23,8 @@ public class SetHandToLevel extends Command {
 
     if (currentPosition > targetPosition) {
       Robot.defenseArm.setHandSpeed(-speed);
-      isDecreasing = true;
     } else {
       Robot.defenseArm.setHandSpeed(speed);
-      isDecreasing = false;
     }
 
   }
@@ -40,11 +37,8 @@ public class SetHandToLevel extends Command {
   protected boolean isFinished() {
     currentPosition = Robot.defenseArm.getHandPotAngle();
 
-    if (isDecreasing == true) {
-      return (currentPosition <= targetPosition + THRESHOLD);
-    } else {
-      return (currentPosition >= targetPosition - THRESHOLD);
-    }
+    double difference = Math.abs(currentPosition - targetPosition);
+    return (difference <= THRESHOLD);
   }
 
   @Override