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 void main(String... args) throws InterruptedException {
init();
long lastVictimTime = 0;
while (true) {
RawColor color = colors.getSensorColor(robot.colorSensor);
if ((color == colors.silver || color == colors.green)
&& System.currentTimeMillis() - lastVictimTime > 2500) {
synchronized (robot.pilot) {
robot.pilot.stop();
robot.showVictimFound();
lastVictimTime = System.currentTimeMillis();
robot.pilot.travel(5);
findline();
}
} else if (color == colors.white || color == colors.black) {
synchronized (robot.pilot) {
int[] sensors = robot.lineSensor.getSensors();
if (sensors != null) {
int leftspeed = calcLeftSpeed(sensors);
int rightspeed = calcRightSpeed(sensors);
robot.pilot.setLeftSpeed(leftspeed);
robot.pilot.setRightSpeed(rightspeed);
}
}
}
Thread.sleep(50);
}
}
public static boolean linefound() {
if (colors.getSensorColor(robot.colorSensor) != colors.white) {
return true;
} else {
/*
* byte[] sensors = robot.lineSensor.getSensors();
* if(sensors != null)
* {
* return sensors[0] < 50 || sensors[3] < 50 || sensors[4] < 50 || sensors[7] < 50;
* }
* else
*/
return false;
}
}