public static void main(String[] args) {
    LCDWriter lcd = LCDWriter.getInstance();
    lcd.start();

    UltrasonicPoller up = UltrasonicPoller.getInstance();
    PathTraveller traveller = PathTraveller.getInstance();

    Odometer odo = Odometer.getInstance();
    Driver dr = Driver.getInstance();

    up.start();
    odo.start();

    traveller.recalculatePathToCoords(135, 105);
    //		traveller.popNextWaypoint();
    //		lcd.writeToScreen(str, lineNumber);
    //		while(!traveller.followThePath()){
    //			traveller.recalculatePathToCoords(75, 75);
    //		}
    boolean done = false;
    while (!done) {
      done = followPath();
      try {
        if (!done) traveller.recalculatePathToCoords(135, 105);
        else break;
      } catch (Exception e) {
        lcd.writeToScreen("E: " + e.toString(), 1);
      }
    }

    // indicate finish
    Sound.beepSequenceUp();
  }
  private static Trajectory searchTile() {
    UltrasonicPoller up = UltrasonicPoller.getInstance();
    NXTRegulatedMotor sensorMotor = Configuration.SENSOR_MOTOR;
    sensorMotor.setSpeed(45);

    sensorMotor.rotateTo(-55, true);
    //		LCDWriter.getInstance().writeToScreen("D: " + up.getDistance(), 0);

    // TODO: better algorithm
    while (sensorMotor.getPosition() > -55) {
      LCDWriter.getInstance().writeToScreen("D: " + up.getDistance(), 0);

      if (up.getDistance() < 25) {
        sensorMotor.rotateTo(0);
        return new Trajectory(-sensorMotor.getPosition() + 15, up.getDistance());
      }
    }

    sensorMotor.rotateTo(55, true);

    while (sensorMotor.getPosition() < 55) {
      LCDWriter.getInstance().writeToScreen("D: " + up.getDistance(), 0);

      if (up.getDistance() < 25) {
        //				sensorMotor.stop();
        sensorMotor.rotateTo(0);
        return new Trajectory(-sensorMotor.getPosition() - 15, up.getDistance());
      }
    }

    return null;
  }
  public static void main(String[] args) {
    LCDWriter lcd = LCDWriter.getInstance();
    lcd.start();

    UltrasonicPoller up = UltrasonicPoller.getInstance();

    Odometer odo = Odometer.getInstance();
    Driver.setSpeed(30);
    Driver dr = Driver.getInstance();
    ObjRec or = new ObjRec();

    up.start();
    odo.start();

    Trajectory block = searchTile();

    //
    lcd.writeToScreen("here", 1);
    if (block != null) {
      dr.turnTo(odo.getTheta() + block.theta);

      dr.forward(Math.abs(block.distance - 5));
      try {
        //				if(or == null) lcd.writeToScreen("NO!", 1);

        ArrayList<ObjRec.blockColor> color = or.detect();

        //
        if (color == null || color.size() == 0) lcd.writeToScreen("EMPTY", 1);
        else {
          int count = 1;
          for (ObjRec.blockColor c : color) if (c != null) lcd.writeToScreen(c.name(), count++);
        }
      } catch (Exception e) {
        lcd.writeToScreen("EXCEPTION", 1);
      }
    }

    //		lcd.writeToScreen("fin", 1);
    // indicate finish
    Sound.beepSequenceUp();
  }
Exemple #4
0
/**
 * test the distance measured by the us sensor and see if it's good
 *
 * @author yuechuan
 */
public class USDTest {

  LCDWriter lcd = LCDWriter.getInstance();
  UltrasonicPoller up = UltrasonicPoller.getInstance();

  public static void main(String[] args) {
    USDTest usdt = new USDTest();
    usdt.lcd.start();
    usdt.up.start();
    while (true) {
      usdt.lcd.writeToScreen(usdt.up.getDistance() + "", 0);
      try {
        Thread.sleep(100);
      } catch (Exception e) {
      }
    }
  }
}