From 822569e25e03cd0c89c2365787bfeb05a77dfb0d Mon Sep 17 00:00:00 2001
From: Meryem Esa <meresa14@gmail.com>
Date: Tue, 16 Feb 2016 11:33:05 -0800
Subject: [PATCH] rewrite MoveIntakeArmToAngle to calculate speed based error

---
 .../intakearm/MoveIntakeArmToAngle.java       | 105 +++++++++---------
 1 file changed, 55 insertions(+), 50 deletions(-)

diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java
index 8541262c..f00e62e1 100644
--- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java
@@ -5,55 +5,60 @@ import org.usfirst.frc.team3501.robot.Robot;
 import edu.wpi.first.wpilibj.command.Command;
 
 public class MoveIntakeArmToAngle extends Command {
-	private double currentAngle;
-	private double targetAngle;
-	private double targetSpeed;
-	private double SENSITIVITY_THRESHOLD = 0.1;
-	private boolean isDecreasing = false;
-
-	public MoveIntakeArmToAngle(double angle, double speed) {
-		requires(Robot.intakeArm);
-		targetAngle = angle;
-		targetSpeed = speed;
-
-	}
-
-	@Override
-	protected void initialize() {
-		currentAngle = Robot.intakeArm.getArmAngle();
-		double difference = targetAngle - currentAngle;
-		if (difference > 0) {
-			Robot.intakeArm.setArmSpeed(targetSpeed);
-			isDecreasing = true;
-		} else {
-			Robot.intakeArm.setArmSpeed(-targetSpeed);
-			isDecreasing = false;
-		}
-	}
-
-	@Override
-	protected void execute() {
-
-	}
-
-	@Override
-	protected boolean isFinished() {
-		currentAngle = Robot.intakeArm.getArmAngle();
-
-		if (isDecreasing == true) {
-			return (currentAngle <= targetAngle + SENSITIVITY_THRESHOLD);
-		} else {
-			return (currentAngle >= targetAngle + SENSITIVITY_THRESHOLD);
-		}
-	}
-
-	@Override
-	protected void end() {
-		Robot.intakeArm.stop();
-	}
-
-	@Override
-	protected void interrupted() {
-	}
+  private double currentAngle;
+  private double targetAngle;
+  private double targetSpeed;
+  private double calculatedSpeed;
+  private double SENSITIVITY_THRESHOLD = 0.1;
+
+  public MoveIntakeArmToAngle(double angle, double speed) {
+    requires(Robot.intakeArm);
+    targetAngle = angle;
+    targetSpeed = speed;
+
+  }
+
+  @Override
+  protected void initialize() {
+
+    // set the arm speed to the calculated angle
+    Robot.intakeArm.setArmSpeed(getCalculatedSpeed());
+
+  }
+
+  @Override
+  protected void execute() {
+    // set the arm speed to the calculated angle
+    Robot.intakeArm.setArmSpeed(calculatedSpeed);
+  }
+
+  private double getError() {
+    // targetAngle - currentAngle = error
+    return Robot.intakeArm.getArmAngle() - targetAngle;
+  }
+
+  private double getCalculatedSpeed() {
+    // if the arm has to move up -- the speed will be +
+    // if the arm has to move down -- the speed will be -
+
+    return (getError() / targetAngle) * targetSpeed;
+  }
+
+  @Override
+  protected boolean isFinished() {
+    // if the arm's angle is close enough to the target value return true
+    // else return false
+    return (Math.abs(getError()) <= SENSITIVITY_THRESHOLD);
+  }
+
+  @Override
+  protected void end() {
+    Robot.intakeArm.stop();
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
 
 }
-- 
2.30.2