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 | ||
9f9d3192 | 7 | public class SetHandToLevel extends Command { |
210e8fe1 SC |
8 | private static final double THRESHOLD = 0.1; |
9 | private double speed; | |
10 | private double targetPosition; | |
11 | private double currentPosition; | |
82505639 | 12 | |
39c9eca8 | 13 | public SetHandToLevel(double speed, int level) { |
82505639 | 14 | requires(Robot.defenseArm); |
210e8fe1 SC |
15 | |
16 | this.speed = speed; | |
39c9eca8 | 17 | this.targetPosition = Robot.defenseArm.getAngleForLocation(level); |
82505639 SC |
18 | } |
19 | ||
20 | @Override | |
21 | protected void initialize() { | |
210e8fe1 SC |
22 | currentPosition = Robot.defenseArm.getHandPotAngle(); |
23 | ||
24 | if (currentPosition > targetPosition) { | |
25 | Robot.defenseArm.setHandSpeed(-speed); | |
210e8fe1 SC |
26 | } else { |
27 | Robot.defenseArm.setHandSpeed(speed); | |
210e8fe1 | 28 | } |
82505639 SC |
29 | |
30 | } | |
31 | ||
32 | @Override | |
33 | protected void execute() { | |
34 | } | |
35 | ||
36 | @Override | |
37 | protected boolean isFinished() { | |
210e8fe1 SC |
38 | currentPosition = Robot.defenseArm.getHandPotAngle(); |
39 | ||
f13765a7 SC |
40 | double difference = Math.abs(currentPosition - targetPosition); |
41 | return (difference <= THRESHOLD); | |
82505639 SC |
42 | } |
43 | ||
44 | @Override | |
45 | protected void end() { | |
210e8fe1 | 46 | Robot.defenseArm.setHandSpeed(0); |
82505639 SC |
47 | } |
48 | ||
49 | @Override | |
50 | protected void interrupted() { | |
51 | end(); | |
52 | } | |
53 | ||
54 | } |