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