From: Shaina Chen <shaina.sierra@gmail.com>
Date: Wed, 10 Feb 2016 04:32:07 +0000 (-0800)
Subject: fill in skeleton for setHandToAngle command
X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=210e8fe10488f062c658b7f93b49c34303aa1705

fill in skeleton for setHandToAngle command
---

diff --git a/src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java
index 6a290111..f7026b47 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java
@@ -5,13 +5,30 @@ import org.usfirst.frc.team3501.robot.Robot;
 import edu.wpi.first.wpilibj.command.Command;
 
 public class SetHandToAngle extends Command {
+  private static final double THRESHOLD = 0.1;
+  private double speed;
+  private double targetPosition;
+  private double currentPosition;
+  private boolean isDecreasing = false;
 
-  public SetHandToAngle() {
+  public SetHandToAngle(double speed, double targetPosition) {
     requires(Robot.defenseArm);
+
+    this.speed = speed;
+    this.targetPosition = targetPosition;
   }
 
   @Override
   protected void initialize() {
+    currentPosition = Robot.defenseArm.getHandPotAngle();
+
+    if (currentPosition > targetPosition) {
+      Robot.defenseArm.setHandSpeed(-speed);
+      isDecreasing = true;
+    } else {
+      Robot.defenseArm.setHandSpeed(speed);
+      isDecreasing = false;
+    }
 
   }
 
@@ -21,11 +38,18 @@ public class SetHandToAngle extends Command {
 
   @Override
   protected boolean isFinished() {
-    return false;
+    currentPosition = Robot.defenseArm.getHandPotAngle();
+
+    if (isDecreasing == true) {
+      return (currentPosition <= targetPosition + THRESHOLD);
+    } else {
+      return (currentPosition >= targetPosition - THRESHOLD);
+    }
   }
 
   @Override
   protected void end() {
+    Robot.defenseArm.setHandSpeed(0);
   }
 
   @Override