Commit | Line | Data |
---|---|---|
69e256f7 SC |
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 | ||
915269db | 7 | public class SetArmToAngle extends Command { |
69e256f7 SC |
8 | private static final double THRESHOLD = 0.1; |
9 | private double speed; | |
10 | private double targetPosition; | |
11 | private double currentPosition; | |
12 | private boolean isDecreasing = false; | |
13 | ||
915269db | 14 | public SetArmToAngle(double speed, double targetPosition) { |
69e256f7 SC |
15 | requires(Robot.defenseArm); |
16 | ||
17 | this.speed = speed; | |
18 | this.targetPosition = targetPosition; | |
19 | } | |
20 | ||
21 | @Override | |
22 | public void initialize() { | |
23 | currentPosition = Robot.defenseArm.getArmPotAngle(); | |
24 | ||
25 | if (currentPosition > targetPosition) { | |
26 | Robot.defenseArm.setArmSpeed(-speed); | |
27 | isDecreasing = true; | |
28 | } else { | |
29 | Robot.defenseArm.setArmSpeed(speed); | |
30 | isDecreasing = false; | |
31 | } | |
32 | } | |
33 | ||
34 | @Override | |
35 | public void execute() { | |
36 | ||
37 | } | |
38 | ||
39 | @Override | |
40 | public boolean isFinished() { | |
41 | currentPosition = Robot.defenseArm.getArmPotAngle(); | |
42 | ||
43 | if (isDecreasing == true) { | |
44 | return (currentPosition <= targetPosition + THRESHOLD); | |
45 | } else { | |
46 | return (currentPosition >= targetPosition - THRESHOLD); | |
47 | } | |
48 | } | |
49 | ||
50 | @Override | |
51 | public void end() { | |
52 | Robot.defenseArm.setArmSpeed(0); | |
53 | } | |
54 | ||
55 | @Override | |
56 | protected void interrupted() { | |
57 | end(); | |
58 | } | |
59 | ||
60 | } |