Commit | Line | Data |
---|---|---|
6bb7f8ac | 1 | package org.usfirst.frc.team3501.robot.commands.driving; |
be737780 | 2 | |
600a1a1c | 3 | import org.usfirst.frc.team3501.robot.Constants; |
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) { | |
600a1a1c | 39 | this(seconds, direction, Constants.Auton.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) { |
6c72e0a8 KZ |
48 | // Robot.driveTrain.drive(speed, -speed); |
49 | Robot.driveTrain.arcadeDrive(0, speed); | |
53924f8d | 50 | } else if (direction == Direction.LEFT) { |
6c72e0a8 KZ |
51 | // Robot.driveTrain.drive(-speed, speed); |
52 | Robot.driveTrain.arcadeDrive(0, speed); | |
53924f8d | 53 | } |
5ac4d78e ME |
54 | } |
55 | ||
4ec3b5b3 ME |
56 | @Override |
57 | protected void execute() { | |
58 | ||
59 | } | |
60 | ||
5ac4d78e ME |
61 | @Override |
62 | protected boolean isFinished() { | |
56a474c2 | 63 | return (timer.get() >= seconds); |
5ac4d78e ME |
64 | } |
65 | ||
66 | @Override | |
67 | protected void end() { | |
18cfb6d8 | 68 | Robot.driveTrain.drive(0, 0); |
5ac4d78e ME |
69 | } |
70 | ||
71 | @Override | |
72 | protected void interrupted() { | |
d113fb3c | 73 | end(); |
5ac4d78e | 74 | } |
be737780 | 75 | } |