Commit | Line | Data |
---|---|---|
44eb0629 KZ |
1 | package org.usfirst.frc3501.RiceCatRobot.commands; |
2 | ||
30a7f376 KZ |
3 | import org.usfirst.frc3501.RiceCatRobot.Robot; |
4 | import org.usfirst.frc3501.RiceCatRobot.RobotMap; | |
5 | ||
44eb0629 KZ |
6 | import edu.wpi.first.wpilibj.command.Command; |
7 | ||
30a7f376 | 8 | public class MoveToLevel extends Command{ |
44eb0629 KZ |
9 | |
10 | int targetLevel, currentLevel; | |
11 | ||
30a7f376 | 12 | public MoveToLevel(int targetLevel) { |
44eb0629 KZ |
13 | this.targetLevel = targetLevel; |
14 | } | |
15 | ||
16 | @Override | |
30a7f376 | 17 | protected void initialize() { |
44eb0629 KZ |
18 | |
19 | } | |
30a7f376 | 20 | |
44eb0629 KZ |
21 | @Override |
22 | protected void execute() { | |
30a7f376 | 23 | Robot.arm.setArmSpeeds(RobotMap.ARM_LOW_SPEED); |
44eb0629 KZ |
24 | } |
25 | ||
26 | @Override | |
30a7f376 KZ |
27 | protected boolean isFinished() { |
28 | if(currentLevel == targetLevel) return true; | |
29 | return false; | |
30 | } | |
44eb0629 KZ |
31 | |
32 | @Override | |
30a7f376 KZ |
33 | protected void end() { |
34 | Robot.arm.setArmSpeeds(0); | |
44eb0629 | 35 | } |
30a7f376 | 36 | |
44eb0629 | 37 | @Override |
30a7f376 KZ |
38 | protected void interrupted() { |
39 | end(); | |
44eb0629 KZ |
40 | } |
41 | ||
42 | } |