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 | 8 | /*** |
1c94f230 KZ |
9 | * @param angle |
10 | * is the setpoint we want to turn for | |
11 | * maxTimeOut catch just in case robot malfunctions and never reaches | |
12 | * setpoint | |
13 | * @initialize resets the Gyro and prints the current mode | |
14 | * @code repeatedly sets a new setpoint angle to the motor | |
15 | * @end ends when the setpoint is reached. | |
337d58e7 | 16 | */ |
be737780 | 17 | public class TurnForAngle extends Command { |
337d58e7 KZ |
18 | private double maxTimeOut; |
19 | double angle; | |
337d58e7 KZ |
20 | |
21 | public TurnForAngle(double angle, double maxTimeOut) { | |
22 | requires(Robot.driveTrain); | |
23 | this.maxTimeOut = maxTimeOut; | |
24 | this.angle = angle; | |
25 | } | |
be737780 | 26 | |
337d58e7 KZ |
27 | @Override |
28 | protected void initialize() { | |
29 | Robot.driveTrain.resetGyro(); | |
30 | System.out.println(Robot.driveTrain.getMode()); | |
31 | } | |
be737780 | 32 | |
337d58e7 KZ |
33 | @Override |
34 | protected void execute() { | |
35 | Robot.driveTrain.turnAngle(angle); | |
36 | Robot.driveTrain.printGyroOutput(); | |
37 | Robot.driveTrain.printOutput(); | |
be737780 | 38 | |
337d58e7 | 39 | } |
be737780 | 40 | |
337d58e7 KZ |
41 | @Override |
42 | protected boolean isFinished() { | |
43 | if (timeSinceInitialized() >= maxTimeOut | |
44 | || Robot.driveTrain | |
45 | .reachedTarget() || Robot.driveTrain.getError() < 8) { | |
46 | System.out.println("time: " + timeSinceInitialized()); | |
47 | Constants.DriveTrain.time = timeSinceInitialized(); | |
48 | return true; | |
49 | } | |
50 | return false; | |
51 | } | |
be737780 | 52 | |
337d58e7 KZ |
53 | @Override |
54 | protected void end() { | |
55 | Robot.driveTrain.disable(); | |
56 | } | |
be737780 | 57 | |
337d58e7 KZ |
58 | @Override |
59 | protected void interrupted() { | |
60 | end(); | |
61 | } | |
be737780 | 62 | } |