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 | ||
f1242e7b ME |
9 | /*** |
10 | * This command turns the robot in a specified direction for a specified | |
4ec3b5b3 | 11 | * duration in seconds. |
f1242e7b ME |
12 | * |
13 | * pre-condition: robot is on a flat surface | |
14 | * | |
15 | * post-condition: robot has turned in the specified direction for the specified | |
16 | * time | |
17 | * | |
18 | * @author Meryem, Avi, and Sarvesh | |
19 | * | |
20 | */ | |
21 | ||
be737780 | 22 | public class TurnForTime extends Command { |
9e3d4f85 | 23 | private static final double DEFAULT_SPEED = 0.5; |
53924f8d ME |
24 | private Direction direction; |
25 | private double seconds; | |
26 | private Timer timer; | |
9e3d4f85 | 27 | private double speed; |
be737780 | 28 | |
9e3d4f85 | 29 | public TurnForTime(double seconds, Direction direction, double speed) { |
6389c060 ME |
30 | this.seconds = seconds; |
31 | this.direction = direction; | |
9e3d4f85 ME |
32 | this.speed = speed; |
33 | } | |
34 | ||
35 | public TurnForTime(double seconds, Direction direction) { | |
36 | this(seconds, direction, DEFAULT_SPEED); | |
5ac4d78e ME |
37 | } |
38 | ||
39 | @Override | |
40 | protected void initialize() { | |
6389c060 ME |
41 | timer = new Timer(); |
42 | timer.start(); | |
5ac4d78e | 43 | |
53924f8d | 44 | if (direction == Direction.RIGHT) { |
9e3d4f85 | 45 | Robot.driveTrain.setMotorSpeeds(speed, -speed); |
53924f8d | 46 | } else if (direction == Direction.LEFT) { |
9e3d4f85 | 47 | Robot.driveTrain.setMotorSpeeds(-speed, speed); |
53924f8d | 48 | } |
5ac4d78e ME |
49 | } |
50 | ||
4ec3b5b3 ME |
51 | @Override |
52 | protected void execute() { | |
53 | ||
54 | } | |
55 | ||
5ac4d78e ME |
56 | @Override |
57 | protected boolean isFinished() { | |
53924f8d ME |
58 | if (timer.get() >= seconds) |
59 | return true; | |
5ac4d78e ME |
60 | return false; |
61 | } | |
62 | ||
63 | @Override | |
64 | protected void end() { | |
f7ab7af3 | 65 | Robot.driveTrain.setMotorSpeeds(0, 0); |
5ac4d78e ME |
66 | } |
67 | ||
68 | @Override | |
69 | protected void interrupted() { | |
70 | } | |
be737780 | 71 | } |