Commit | Line | Data |
---|---|---|
6bb7f8ac | 1 | package org.usfirst.frc.team3501.robot.commands.driving; |
be737780 | 2 | |
5ac4d78e | 3 | import org.usfirst.frc.team3501.robot.Constants.Direction; |
53924f8d | 4 | import org.usfirst.frc.team3501.robot.Robot; |
5ac4d78e | 5 | |
de0fd5e3 | 6 | import edu.wpi.first.wpilibj.Timer; |
be737780 ME |
7 | import edu.wpi.first.wpilibj.command.Command; |
8 | ||
be737780 | 9 | public class TurnForTime extends Command { |
53924f8d ME |
10 | private final double SPEED = 0.5; |
11 | private Direction direction; | |
12 | private double seconds; | |
13 | private Timer timer; | |
be737780 | 14 | |
5ac4d78e | 15 | public TurnForTime(double seconds, Direction direction) { |
6389c060 ME |
16 | this.seconds = seconds; |
17 | this.direction = direction; | |
5ac4d78e ME |
18 | } |
19 | ||
20 | @Override | |
21 | protected void initialize() { | |
6389c060 ME |
22 | timer = new Timer(); |
23 | timer.start(); | |
5ac4d78e ME |
24 | } |
25 | ||
26 | @Override | |
27 | protected void execute() { | |
53924f8d ME |
28 | |
29 | if (direction == Direction.RIGHT) { | |
30 | Robot.driveTrain.setMotorSpeeds(SPEED, -SPEED); | |
31 | } else if (direction == Direction.LEFT) { | |
32 | Robot.driveTrain.setMotorSpeeds(-SPEED, SPEED); | |
33 | } | |
5ac4d78e ME |
34 | } |
35 | ||
36 | @Override | |
37 | protected boolean isFinished() { | |
53924f8d ME |
38 | if (timer.get() >= seconds) |
39 | return true; | |
5ac4d78e ME |
40 | return false; |
41 | } | |
42 | ||
43 | @Override | |
44 | protected void end() { | |
45 | } | |
46 | ||
47 | @Override | |
48 | protected void interrupted() { | |
49 | } | |
be737780 | 50 | } |