Commit | Line | Data |
---|---|---|
0d47db19 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 | ||
b007b3f6 SC |
7 | /*** |
8 | * This command moves the defense arm to a input angle position. | |
9 | * Requires input of a targetPosition and a speed. | |
10 | * | |
11 | * @author shaina | |
12 | * | |
13 | */ | |
0d47db19 SC |
14 | public class SetHandToAngle extends Command { |
15 | private static final double THRESHOLD = 0.1; | |
16 | private double speed; | |
17 | private double targetPosition; | |
18 | private double currentPosition; | |
19 | private boolean isDecreasing = false; | |
20 | ||
21 | public SetHandToAngle(double speed, double targetPosition) { | |
22 | requires(Robot.defenseArm); | |
23 | ||
24 | this.speed = speed; | |
25 | this.targetPosition = targetPosition; | |
26 | } | |
27 | ||
28 | @Override | |
29 | public void initialize() { | |
30 | currentPosition = Robot.defenseArm.getHandPotAngle(); | |
31 | ||
32 | if (currentPosition > targetPosition) { | |
33 | Robot.defenseArm.setArmSpeed(-speed); | |
34 | isDecreasing = true; | |
35 | } else { | |
36 | Robot.defenseArm.setArmSpeed(speed); | |
37 | isDecreasing = false; | |
38 | } | |
39 | } | |
40 | ||
41 | @Override | |
42 | public void execute() { | |
43 | ||
44 | } | |
45 | ||
46 | @Override | |
47 | public boolean isFinished() { | |
48 | currentPosition = Robot.defenseArm.getHandPotAngle(); | |
49 | ||
50 | if (isDecreasing == true) { | |
51 | return (currentPosition <= targetPosition + THRESHOLD); | |
52 | } else { | |
53 | return (currentPosition >= targetPosition - THRESHOLD); | |
54 | } | |
55 | } | |
56 | ||
57 | @Override | |
58 | public void end() { | |
59 | Robot.defenseArm.setArmSpeed(0); | |
60 | } | |
61 | ||
62 | @Override | |
63 | protected void interrupted() { | |
64 | end(); | |
65 | } | |
66 | ||
67 | } |