Commit | Line | Data |
---|---|---|
6bb7f8ac | 1 | package org.usfirst.frc.team3501.robot.commands.driving; |
be737780 | 2 | |
337d58e7 KZ |
3 | import org.usfirst.frc.team3501.robot.Constants; |
4 | import org.usfirst.frc.team3501.robot.Robot; | |
5 | ||
be737780 ME |
6 | import edu.wpi.first.wpilibj.command.Command; |
7 | ||
337d58e7 KZ |
8 | /*** |
9 | * This command will move the robot at the specified speed for the specified | |
10 | * distance. | |
11 | * | |
12 | * post-condition: has driven for the distance and speed specified | |
13 | * | |
14 | * @author Meryem and Avi | |
15 | * | |
16 | */ | |
be737780 | 17 | public class TurnForAngle extends Command { |
337d58e7 KZ |
18 | private double maxTimeOut; |
19 | double angle; | |
20 | int count = 0; | |
21 | ||
22 | public TurnForAngle(double angle, double maxTimeOut) { | |
23 | requires(Robot.driveTrain); | |
24 | this.maxTimeOut = maxTimeOut; | |
25 | this.angle = angle; | |
26 | } | |
be737780 | 27 | |
337d58e7 KZ |
28 | @Override |
29 | protected void initialize() { | |
30 | Robot.driveTrain.resetGyro(); | |
31 | System.out.println(Robot.driveTrain.getMode()); | |
32 | } | |
be737780 | 33 | |
337d58e7 KZ |
34 | @Override |
35 | protected void execute() { | |
36 | Robot.driveTrain.turnAngle(angle); | |
37 | Robot.driveTrain.printGyroOutput(); | |
38 | Robot.driveTrain.printOutput(); | |
39 | count++; | |
be737780 | 40 | |
337d58e7 | 41 | } |
be737780 | 42 | |
337d58e7 KZ |
43 | @Override |
44 | protected boolean isFinished() { | |
45 | if (timeSinceInitialized() >= maxTimeOut | |
46 | || Robot.driveTrain | |
47 | .reachedTarget() || Robot.driveTrain.getError() < 8) { | |
48 | System.out.println("time: " + timeSinceInitialized()); | |
49 | Constants.DriveTrain.time = timeSinceInitialized(); | |
50 | return true; | |
51 | } | |
52 | return false; | |
53 | } | |
be737780 | 54 | |
337d58e7 KZ |
55 | @Override |
56 | protected void end() { | |
57 | Robot.driveTrain.disable(); | |
58 | } | |
be737780 | 59 | |
337d58e7 KZ |
60 | @Override |
61 | protected void interrupted() { | |
62 | end(); | |
63 | } | |
be737780 | 64 | } |