From 6c101902ba98914df241a156fb223da6a994cc09 Mon Sep 17 00:00:00 2001
From: Shaina Chen <shaina.sierra@gmail.com>
Date: Wed, 10 Feb 2016 20:34:01 -0800
Subject: [PATCH] change command to moving to level instead of target angle and
 simplify isFinished

---
 .../frc/team3501/robot/commands/SetHandToLevel.java    | 10 ++--------
 1 file changed, 2 insertions(+), 8 deletions(-)

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
-- 
2.30.2