From 0d47db19f0bddcf49b43e6a57478c3cd6a24ac72 Mon Sep 17 00:00:00 2001
From: Shaina Chen <shaina.sierra@gmail.com>
Date: Thu, 11 Feb 2016 19:08:29 -0800
Subject: [PATCH] create and fill in setHandToAngle command and change variable
 name for setHandToAngle in retractDefenseArm

---
 .../robot/commands/RetractDefenseArm.java     |  2 +-
 .../robot/commands/SetHandToAngle.java        | 60 +++++++++++++++++++
 2 files changed, 61 insertions(+), 1 deletion(-)
 create mode 100755 src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java b/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java
index a3cb6d45..8d530bee 100755
--- a/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java
@@ -10,6 +10,6 @@ public class RetractDefenseArm extends CommandGroup {
     requires(Robot.defenseArm);
 
     addParallel(new SetArmToAngle(speed, retractPosition));
-    addParallel(new SetHandToLevel(speed, retractPosition));
+    addParallel(new SetHandToAngle(speed, retractPosition));
   }
 }
diff --git a/src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java
new file mode 100755
index 00000000..d72db43d
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java
@@ -0,0 +1,60 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+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(double speed, double targetPosition) {
+    requires(Robot.defenseArm);
+
+    this.speed = speed;
+    this.targetPosition = targetPosition;
+  }
+
+  @Override
+  public void initialize() {
+    currentPosition = Robot.defenseArm.getHandPotAngle();
+
+    if (currentPosition > targetPosition) {
+      Robot.defenseArm.setArmSpeed(-speed);
+      isDecreasing = true;
+    } else {
+      Robot.defenseArm.setArmSpeed(speed);
+      isDecreasing = false;
+    }
+  }
+
+  @Override
+  public void execute() {
+
+  }
+
+  @Override
+  public boolean isFinished() {
+    currentPosition = Robot.defenseArm.getHandPotAngle();
+
+    if (isDecreasing == true) {
+      return (currentPosition <= targetPosition + THRESHOLD);
+    } else {
+      return (currentPosition >= targetPosition - THRESHOLD);
+    }
+  }
+
+  @Override
+  public void end() {
+    Robot.defenseArm.setArmSpeed(0);
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+
+}
-- 
2.30.2