Commit | Line | Data |
---|---|---|
f11ce98e KZ |
1 | package org.usfirst.frc3501.RiceCatRobot.commands; |
2 | ||
4b8a525f KZ |
3 | import org.usfirst.frc3501.RiceCatRobot.robot.Robot; |
4 | import org.usfirst.frc3501.RiceCatRobot.robot.RobotMap.Direction; | |
f11ce98e KZ |
5 | |
6 | import edu.wpi.first.wpilibj.Timer; | |
7 | import edu.wpi.first.wpilibj.command.Command; | |
8 | ||
9 | /** | |
10 | * This command takes a time in seconds which is how long it should run | |
11 | * | |
12 | */ | |
13 | public class DriveFor extends Command { | |
e3bfff96 KZ |
14 | private double seconds; |
15 | private Timer timer; | |
16 | private Direction direction; | |
f11ce98e | 17 | |
e3bfff96 KZ |
18 | public DriveFor(double seconds, Direction direction) { |
19 | this.seconds = seconds; | |
20 | this.direction = direction; | |
f11ce98e | 21 | |
e3bfff96 | 22 | } |
f11ce98e | 23 | |
e3bfff96 KZ |
24 | @Override |
25 | protected void initialize() { | |
26 | timer = new Timer(); | |
27 | timer.reset(); | |
28 | timer.start(); | |
29 | } | |
f11ce98e | 30 | |
e3bfff96 KZ |
31 | @Override |
32 | protected void execute() { | |
33 | System.out.println(timer.get()); | |
34 | if (direction == Direction.FORWARD) { | |
35 | if (timer.get() < seconds * 0.2) { // for the first 20% of time, run | |
36 | // the robot at -.5 speed | |
37 | Robot.driveTrain.arcadeDrive(-0.3, 0); | |
38 | } else if (timer.get() >= seconds * 0.2 && timer.get() <= seconds * 0.8) { | |
39 | Robot.driveTrain.arcadeDrive(-0.5, 0); | |
40 | } else if (timer.get() < seconds) { | |
41 | Robot.driveTrain.arcadeDrive(-0.25, 0); | |
42 | } else { | |
43 | Robot.driveTrain.arcadeDrive(0, 0); | |
44 | } | |
45 | } else if (direction == Direction.BACKWARD) { | |
46 | if (timer.get() < seconds * 0.2) { | |
47 | Robot.driveTrain.arcadeDrive(0.3, 0); | |
48 | } else if (timer.get() >= seconds * 0.2 && timer.get() <= seconds * 0.8) { | |
49 | Robot.driveTrain.arcadeDrive(0.5, 0); | |
50 | } else if (timer.get() < seconds) { | |
51 | Robot.driveTrain.arcadeDrive(0.25, 0); | |
52 | } else { | |
53 | Robot.driveTrain.arcadeDrive(0, 0); | |
54 | } | |
f11ce98e | 55 | } |
e3bfff96 | 56 | } |
f11ce98e | 57 | |
e3bfff96 KZ |
58 | @Override |
59 | protected boolean isFinished() { | |
60 | return timer.get() > seconds; | |
61 | } | |
f11ce98e | 62 | |
e3bfff96 KZ |
63 | @Override |
64 | protected void end() { | |
65 | Robot.driveTrain.arcadeDrive(0, 0); | |
66 | } | |
f11ce98e | 67 | |
e3bfff96 KZ |
68 | @Override |
69 | protected void interrupted() { | |
70 | end(); | |
71 | } | |
b72e169c | 72 | } |