X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fdoublejointeddefensearm%2FSetHandToAngle.java;fp=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fcommands%2Fdoublejointeddefensearm%2FSetHandToAngle.java;h=98f306a57e29f892e669c4bc8fed3ac40cb304f6;hb=60fa3e53be375f99b4a5f5d97515987db1905935;hp=0000000000000000000000000000000000000000;hpb=643241dab07791483b0b8d33b38362c065d31cf3;p=3501%2Fstronghold-2016

diff --git a/src/org/usfirst/frc/team3501/robot/commands/doublejointeddefensearm/SetHandToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/doublejointeddefensearm/SetHandToAngle.java
new file mode 100755
index 00000000..98f306a5
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/doublejointeddefensearm/SetHandToAngle.java
@@ -0,0 +1,67 @@
+package org.usfirst.frc.team3501.robot.commands.doublejointeddefensearm;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/***
+ * This command moves the defense arm to a input angle position.
+ * Requires input of a targetPosition and a speed.
+ * 
+ * @author shaina
+ *
+ */
+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();
+  }
+
+}