rewrite MoveIntakeArmToAngle to calculate speed based error

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 8541262cedbfaccea750ee77031b38b1917414b9..f00e62e1984bb4dc49b682bc2b911c118f8c27b2 100644 (file)
--- 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();
+  }
 
 }