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 ME |
48 | Robot.driveTrain.drive(speed, -speed); |
49 | ; | |
53924f8d | 50 | } else if (direction == Direction.LEFT) { |
9e3d4f85 | 51 | Robot.driveTrain.setMotorSpeeds(-speed, speed); |
53924f8d | 52 | } |
5ac4d78e ME |
53 | } |
54 | ||
4ec3b5b3 ME |
55 | @Override |
56 | protected void execute() { | |
57 | ||
58 | } | |
59 | ||
5ac4d78e ME |
60 | @Override |
61 | protected boolean isFinished() { | |
56a474c2 | 62 | return (timer.get() >= seconds); |
5ac4d78e ME |
63 | } |
64 | ||
65 | @Override | |
66 | protected void end() { | |
18cfb6d8 | 67 | Robot.driveTrain.drive(0, 0); |
5ac4d78e ME |
68 | } |
69 | ||
70 | @Override | |
71 | protected void interrupted() { | |
72 | } | |
be737780 | 73 | } |