Commit | Line | Data |
---|---|---|
f53566b3 HD |
1 | package org.usfirst.frc.team3501.robot.commands.driving; |
2 | ||
3 | import org.usfirst.frc.team3501.robot.Constants.Auton; | |
4 | import org.usfirst.frc.team3501.robot.Robot; | |
5 | ||
6 | import edu.wpi.first.wpilibj.Timer; | |
7 | import edu.wpi.first.wpilibj.command.Command; | |
8 | ||
9 | public class TimeDrive extends Command { | |
10 | Timer timer; | |
11 | double currentTime, targetTime, speed; | |
12 | ||
13 | public TimeDrive() { | |
14 | this(Auton.DEFAULT_TIME, Auton.DEFAULT_SPEED); | |
15 | } | |
16 | ||
17 | public TimeDrive(double time) { | |
18 | this(time, Auton.DEFAULT_SPEED); | |
19 | } | |
20 | ||
21 | public TimeDrive(double time, double speed) { | |
22 | requires(Robot.driveTrain); | |
23 | ||
24 | timer = new Timer(); | |
25 | this.currentTime = 0; | |
26 | this.targetTime = time; | |
27 | this.speed = speed; | |
28 | } | |
29 | ||
30 | @Override | |
31 | protected void initialize() { | |
32 | timer.start(); | |
33 | } | |
34 | ||
35 | @Override | |
36 | protected void execute() { | |
37 | currentTime = timer.get(); | |
38 | ||
39 | double output = speed * ((targetTime - currentTime) / (targetTime)); | |
40 | ||
41 | Robot.driveTrain.setMotorSpeeds(output, output); | |
42 | } | |
43 | ||
44 | @Override | |
45 | protected boolean isFinished() { | |
46 | return currentTime >= targetTime; | |
47 | } | |
48 | ||
49 | @Override | |
50 | protected void end() { | |
51 | Robot.driveTrain.stop(); | |
52 | } | |
53 | ||
54 | @Override | |
55 | protected void interrupted() { | |
56 | end(); | |
57 | } | |
58 | } |