Commit | Line | Data |
---|---|---|
f11ce98e KZ |
1 | package org.usfirst.frc3501.RiceCatRobot.commands; |
2 | ||
3 | import org.usfirst.frc3501.RiceCatRobot.Robot; | |
4 | import org.usfirst.frc3501.RiceCatRobot.RobotMap; | |
5 | import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction; | |
6 | ||
7 | import edu.wpi.first.wpilibj.Timer; | |
8 | import edu.wpi.first.wpilibj.command.Command; | |
9 | ||
10 | /** | |
11 | * This command takes a time in seconds which is how long it should run to move | |
12 | * arm up or down | |
13 | * | |
14 | */ | |
15 | public class MoveArmFor extends Command { | |
16 | private double seconds; | |
17 | private Timer timer; | |
18 | private Direction direction; | |
19 | ||
20 | /* | |
21 | * @param Direction must be up or down | |
22 | */ | |
23 | public MoveArmFor(double seconds, Direction direction) { | |
24 | this.seconds = seconds; | |
25 | this.direction = direction; | |
26 | } | |
27 | ||
28 | @Override | |
29 | protected void initialize() { | |
30 | timer = new Timer(); | |
31 | timer.start(); | |
32 | } | |
33 | ||
34 | @Override | |
35 | protected void execute() { | |
36 | if (direction == Direction.UP) { | |
37 | Robot.arm.setArmSpeeds(-RobotMap.ARM_LOW_SPEED); | |
38 | } else if (direction == Direction.DOWN) { | |
39 | Robot.arm.setArmSpeeds(RobotMap.ARM_LOW_SPEED); | |
40 | } | |
41 | } | |
42 | ||
43 | @Override | |
44 | protected boolean isFinished() { | |
45 | if (timer.get() > seconds) { | |
46 | Robot.arm.setArmSpeeds(0); | |
47 | } | |
48 | return timer.get() > seconds; | |
49 | } | |
50 | ||
51 | @Override | |
52 | protected void end() { | |
53 | Robot.arm.setArmSpeeds(0); | |
54 | } | |
55 | ||
56 | @Override | |
57 | protected void interrupted() { | |
58 | end(); | |
59 | } | |
f11ce98e | 60 | } |