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