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