| 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 | } |