예제 #1
0
  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();
  }
예제 #2
0
  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();
  }
예제 #3
0
  /** Run a search routine */
  public static void searchForBlock() {
    Stack<Coordinate> path = Searcher.generateSearchPath();
    Configuration conf = Configuration.getInstance();
    Driver driver = Driver.getInstance();
    Coordinate first = path.peek();
    LCDWriter lcd = LCDWriter.getInstance();
    CaptureMechanism cm = CaptureMechanism.getInstance();

    int BLOCK_COLOR = conf.getBlockColor().getCode();

    boolean blockFound = false;

    Configuration.getInstance().setForwardSpeed(300);
    Configuration.getInstance().setRotationSpeed(250);

    while (!blockFound && !path.isEmpty()) {
      Coordinate p = path.pop();
      lcd.writeToScreen("Des:" + p.toString(), 4);
      driver.turnTo(Coordinate.calculateRotationAngle(conf.getCurrentLocation(), p));
      int result = ObjectDetector.lookForItemII(BLOCK_COLOR);
      if (result == 1) {
        Sound.beep();
        blockFound = true;
        cm.open();
        driver.forward(15);
        cm.align();
        driver.forward(15);
        cm.close();
        Sound.beep();
        Sound.beepSequenceUp();
        break;
      }
      // If not the right block remove it
      else if (result == 0 && Searcher.inSearchZone()) {
        cm.removeBlockII();

        ObjRec oRec = new ObjRec();
        // test again
        ArrayList<blockColor> bc = oRec.detect();
        String ans = "";
        for (blockColor b : bc) {
          ans += b.toString();
        }
        lcd.writeToScreen(ans, 6);
        if (bc.contains(blockColor.getInstance(BLOCK_COLOR))) {
          // face the robot
          blockFound = true;
          cm.open();
          driver.forward(15);
          cm.align();
          driver.forward(15);
          cm.close();
          Sound.beep();
          Sound.beepSequenceUp();
          break;
        }
      }

      driver.travelTo(p);

      // Double check for item
      result = ObjectDetector.lookForItemII(BLOCK_COLOR);
      // capture it
      if (result == 1) {
        Sound.beep();
        blockFound = true;
        cm.open();
        driver.forward(15);
        cm.align();
        driver.forward(15);
        cm.close();
        Sound.beep();
        Sound.beepSequenceUp();
        break;
      } else if (result == 0 && Searcher.inSearchZone()) {
        cm.removeBlockII();
        ObjRec oRec = new ObjRec();
        // test again
        ArrayList<blockColor> bc = oRec.detect();
        String ans = "";
        for (blockColor b : bc) {
          ans += b.toString();
        }
        lcd.writeToScreen(ans, 6);
        if (bc.contains(blockColor.getInstance(BLOCK_COLOR))) {
          // face the robot
          blockFound = true;
          cm.open();
          driver.forward(15);
          cm.align();
          driver.forward(15);
          cm.close();
          Sound.beep();
          Sound.beepSequenceUp();
          break;
        }
      }

      if (path.isEmpty() && !blockFound) {
        path = Searcher.generateSearchPath();
      }
    }
  }