Commit | Line | Data |
---|---|---|
6bb7f8ac | 1 | package org.usfirst.frc.team3501.robot.commands.driving; |
be737780 | 2 | |
585b7a9e | 3 | import org.usfirst.frc.team3501.robot.Constants.DeadReckoning; |
5ac4d78e | 4 | import org.usfirst.frc.team3501.robot.Constants.Direction; |
53924f8d | 5 | import org.usfirst.frc.team3501.robot.Robot; |
5ac4d78e | 6 | |
de0fd5e3 | 7 | import edu.wpi.first.wpilibj.Timer; |
be737780 ME |
8 | import edu.wpi.first.wpilibj.command.Command; |
9 | ||
f1242e7b ME |
10 | /*** |
11 | * This command turns the robot in a specified direction for a specified | |
4ec3b5b3 | 12 | * duration in seconds. |
f1242e7b ME |
13 | * |
14 | * pre-condition: robot is on a flat surface | |
15 | * | |
16 | * post-condition: robot has turned in the specified direction for the specified | |
17 | * time | |
18 | * | |
3b2cd17d ME |
19 | * TODO: test for speed/ time constants for specific angles (ex. 30 degrees, 60 |
20 | * degrees, 90 degrees) | |
56a474c2 | 21 | * |
f1242e7b ME |
22 | * @author Meryem, Avi, and Sarvesh |
23 | * | |
24 | */ | |
25 | ||
be737780 | 26 | public class TurnForTime extends Command { |
53924f8d ME |
27 | private Direction direction; |
28 | private double seconds; | |
29 | private Timer timer; | |
9e3d4f85 | 30 | private double speed; |
be737780 | 31 | |
9e3d4f85 | 32 | public TurnForTime(double seconds, Direction direction, double speed) { |
6389c060 ME |
33 | this.seconds = seconds; |
34 | this.direction = direction; | |
9e3d4f85 ME |
35 | this.speed = speed; |
36 | } | |
37 | ||
38 | public TurnForTime(double seconds, Direction direction) { | |
585b7a9e | 39 | this(seconds, direction, DeadReckoning.DEFAULT_SPEED); |
5ac4d78e ME |
40 | } |
41 | ||
42 | @Override | |
43 | protected void initialize() { | |
6389c060 ME |
44 | timer = new Timer(); |
45 | timer.start(); | |
5ac4d78e | 46 | |
53924f8d | 47 | if (direction == Direction.RIGHT) { |
18cfb6d8 | 48 | Robot.driveTrain.drive(speed, -speed); |
53924f8d | 49 | } else if (direction == Direction.LEFT) { |
41f93111 | 50 | Robot.driveTrain.drive(-speed, speed); |
53924f8d | 51 | } |
5ac4d78e ME |
52 | } |
53 | ||
4ec3b5b3 ME |
54 | @Override |
55 | protected void execute() { | |
56 | ||
57 | } | |
58 | ||
5ac4d78e ME |
59 | @Override |
60 | protected boolean isFinished() { | |
56a474c2 | 61 | return (timer.get() >= seconds); |
5ac4d78e ME |
62 | } |
63 | ||
64 | @Override | |
65 | protected void end() { | |
18cfb6d8 | 66 | Robot.driveTrain.drive(0, 0); |
5ac4d78e ME |
67 | } |
68 | ||
69 | @Override | |
70 | protected void interrupted() { | |
d113fb3c | 71 | end(); |
5ac4d78e | 72 | } |
be737780 | 73 | } |