Commit | Line | Data |
---|---|---|
6bb7f8ac | 1 | package org.usfirst.frc.team3501.robot.commands.intakearm; |
b4eeeb65 SO |
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 { | |
822569e2 ME |
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 | } | |
b4eeeb65 SO |
63 | |
64 | } |