challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java

   1 package org.usfirst.frc.team3501.robot.commands.intakearm;
   2
   3 import org.usfirst.frc.team3501.robot.Robot;
   4
   5 import edu.wpi.first.wpilibj.command.Command;
   6
   7 public class MoveIntakeArmToAngle extends Command {
   8   private double currentAngle;
   9   private double targetAngle;
  10   private double targetSpeed;
  11   private double calculatedSpeed;
  12   private double SENSITIVITY_THRESHOLD = 0.1;
  13
  14   public MoveIntakeArmToAngle(double angle, double speed) {
  15     requires(Robot.intakeArm);
  16     targetAngle = angle;
  17     targetSpeed = speed;
  18
  19   }
  20
  21   @Override
  22   protected void initialize() {
  23
  24     // set the arm speed to the calculated angle
  25     Robot.intakeArm.setArmSpeed(getCalculatedSpeed());
  26
  27   }
  28
  29   @Override
  30   protected void execute() {
  31     // set the arm speed to the calculated angle
  32     Robot.intakeArm.setArmSpeed(calculatedSpeed);
  33   }
  34
  35   private double getError() {
  36     // targetAngle - currentAngle = error
  37     return Robot.intakeArm.getArmAngle() - targetAngle;
  38   }
  39
  40   private double getCalculatedSpeed() {
  41     // if the arm has to move up -- the speed will be +
  42     // if the arm has to move down -- the speed will be -
  43
  44     return (getError() / targetAngle) * targetSpeed;
  45   }
  46
  47   @Override
  48   protected boolean isFinished() {
  49     // if the arm's angle is close enough to the target value return true
  50     // else return false
  51     return (Math.abs(getError()) <= SENSITIVITY_THRESHOLD);
  52   }
  53
  54   @Override
  55   protected void end() {
  56     Robot.intakeArm.stop();
  57   }
  58
  59   @Override
  60   protected void interrupted() {
  61     end();
  62   }
  63
  64 }