protected void execute() {
if (complete) {
return;
}
if (!cam.isProccessingCamera()) {
Robot.driveSubsystem.tankdrive(0, 0);
lastTime = System.currentTimeMillis(); // Wait for the camera to switch over.
return;
}
if (lastTime + 700
> System.currentTimeMillis()) { // Wait for the camera to actually update the images.
Robot.driveSubsystem.tankdrive(0, 0);
return;
}
double goalY = cam.getGoalXY()[1];
if (goalY != -1.0) {
if (goalY > (double) (center + calibration)) {
Robot.driveSubsystem.tankdrive(0.5, 0.5);
} else if (goalY < (double) (center - calibration)) {
Robot.driveSubsystem.tankdrive(-0.5, -0.5);
} else {
complete = true;
Logger.out("So the camera ended here: " + goalY);
}
} else {
if (errors++ > CameraConstants.MAX_CAMERA_MISSES) {
Logger.err("Quit because I could not find a goal! Has the robot been rotated first?");
Robot.STOP_AUTO = "Can't find goal to drive!";
complete = true;
}
}
}
protected void end() {
Robot.driveSubsystem.tankdrive(0, 0);
if (cam != null) {
cam.controllerable(true);
cam.switchCam(Cam.FRONT_CAM);
} else {
Logger.err("The camera isn't a thing, yo.");
}
}
protected void initialize() {
complete = false;
if (Robot.AUTOING && Robot.STOP_AUTO != null) {
complete = true;
return;
}
calibration = Util.setAndGetNumber("FWD", "Calibration Value", 10);
center = Util.setAndGetNumber("CAM", "Cross Y", 135) / 2; // Image is half normal
if (cam != null) {
cam.controllerable(false);
cam.switchCam(Cam.FRONT_PROCESSED);
} else {
Logger.err("The camera isn't a thing, yo.");
}
errors = 0;
}
protected void interrupted() {
Logger.err("You can't just interrupt the camera like that! ... Ok, maybe you can.");
end();
}