Commit | Line | Data |
---|---|---|
b4eeeb65 SO |
1 | package org.usfirst.frc.team3501.robot.commands; |
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 SENSITIVITY_THRESHOLD = 0.1; | |
12 | private boolean isDecreasing = false; | |
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 | currentAngle = Robot.intakeArm.getArmAngle(); | |
24 | double difference = targetAngle - currentAngle; | |
25 | if (difference > 0) { | |
26 | Robot.intakeArm.setArmSpeed(targetSpeed); | |
27 | isDecreasing = true; | |
28 | } else { | |
29 | Robot.intakeArm.setArmSpeed(-targetSpeed); | |
30 | isDecreasing = false; | |
31 | } | |
32 | } | |
33 | ||
34 | @Override | |
35 | protected void execute() { | |
36 | ||
37 | } | |
38 | ||
39 | @Override | |
40 | protected boolean isFinished() { | |
41 | currentAngle = Robot.intakeArm.getArmAngle(); | |
42 | ||
43 | if (isDecreasing == true) { | |
44 | return (currentAngle <= targetAngle + SENSITIVITY_THRESHOLD); | |
45 | } else { | |
46 | return (currentAngle >= targetAngle + SENSITIVITY_THRESHOLD); | |
47 | } | |
48 | } | |
49 | ||
50 | @Override | |
51 | protected void end() { | |
52 | Robot.intakeArm.stop(); | |
53 | } | |
54 | ||
55 | @Override | |
56 | protected void interrupted() { | |
57 | } | |
58 | ||
59 | } |