Total Complexity | 7 |
Total Lines | 33 |
Duplicated Lines | 0 % |
Changes | 3 | ||
Bugs | 0 | Features | 0 |
1 | package org.usfirst.frc.team3695.robot.commands; |
||
8 | public class CyborgCommandDriveUntilError extends Command { |
||
9 | public static final long ERROR_TIME = 500; |
||
10 | public static final int TARGET_ERROR = 500; |
||
11 | |||
12 | private long time = 0; |
||
|
|||
13 | |||
14 | public CyborgCommandDriveUntilError() { |
||
15 | requires(Robot.SUB_DRIVE); |
||
16 | } |
||
17 | |||
18 | protected void initialize() { |
||
19 | time = System.currentTimeMillis() + ERROR_TIME; |
||
20 | } |
||
21 | |||
22 | protected void execute() { |
||
23 | double speed = Util.getAndSetDouble("SPEED ERROR: Forward", -0.25); |
||
24 | Robot.SUB_DRIVE.driveDirect(speed, speed); |
||
25 | } |
||
26 | |||
27 | protected boolean isFinished() { |
||
28 | if(Math.abs(Robot.SUB_DRIVE.pid.getError()) < TARGET_ERROR) { |
||
29 | time = System.currentTimeMillis() + ERROR_TIME; |
||
30 | } |
||
31 | return time < System.currentTimeMillis(); |
||
32 | } |
||
33 | |||
34 | protected void end() { |
||
35 | DriverStation.reportWarning("CyborgCommandDriveUntilError finished", false); |
||
36 | Robot.SUB_DRIVE.driveDirect(0, 0); |
||
37 | } |
||
38 | |||
39 | protected void interrupted() { |
||
40 | end(); |
||
41 | } |
||
42 | } |