Ejemplo n.º 1
0
  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;
  }
Ejemplo n.º 2
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();
  }
Ejemplo n.º 3
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) {
      }
    }
  }
}
Ejemplo n.º 4
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();
  }
Ejemplo n.º 5
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();
      }
    }
  }