static void init() throws InterruptedException {
Sound.beep();
try {
File f = new File("CalibrationData.dat");
FileInputStream is = new FileInputStream(f);
colors = RescueColors.readObject(is);
is.close();
LCD.drawString("File read", 0, 0);
LCD.drawString("sucessfully", 0, 0);
} catch (IOException e) {
LCD.drawString("Error reading", 0, 0);
LCD.drawString("file", 0, 1);
Thread.sleep(1000);
System.exit(-1);
}
colors.printToLCD();
robot = new RescueRobot(colors);
Button.ENTER.waitForPressAndRelease();
logger = new MovementLogger(robot);
Thread t = new Thread(logger);
t.setDaemon(true);
t.start();
}
public static boolean findline() {
TachoPilot p = robot.pilot;
int[] angles = new int[] {30, -30, 60, -60, 90, -90, 0};
int lastangle = 0;
for (int angle : angles) {
p.rotate((lastangle - angle) * 4, true);
while (p.isMoving()) {
if (linefound()) {
Sound.beepSequence();
p.stop();
return true;
}
Thread.yield();
}
p.stop();
lastangle = angle;
Delay.msDelay(250);
}
return false;
}