From: Shaina Chen <shaina.sierra@gmail.com>
Date: Thu, 11 Feb 2016 04:34:01 +0000 (-0800)
Subject: change command to moving to level instead of target angle and simplify isFinished
X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=6c101902ba98914df241a156fb223da6a994cc09;p=3501%2Fstronghold-2016

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 11aae28a..3d7fb748 100755
--- 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