예제 #1
0
  public static void Measurement() {
    // execute all actions of this state
    LCD.drawString("Measurement", 0, 3);
    btThread.popElement();
    Sound.beep();
    Sound.beep();
    // lower sensor by RCX motor
    tempMotor.setPower(-100);
    tempMotor.setPower(0);
    for (int i = 0; i < nrcolors; i++) {
      if (colorsens.getColorID() == lakes[i].color && !lakes[i].found) // kleuren komen overeen
      lakes[i].celsius = tempSensor.getCelcius();
      lakes[i].found = true;
      return;
    }
    // raise temp sensor
    tempMotor.setPower(100);
    btThread.write(500); // sends the message 'action done'

    // leg de huidige tijd vast voor alle transitions met een timeoutcondition
    long starttime = System.currentTimeMillis();

    // when done, wait for a trigger for a transition
    boolean transitionTaken = false;
    while (!transitionTaken) {
      if ((lakes[0].found && lakes[1].found && lakes[2].found)) {
        current = State.FINISHED;
        transitionTaken = true;
      } else if ((true)) {
        current = State.WATCH;
        transitionTaken = true;
      }
    }
  }
  /**
   * Calibrates a single axis. The calibration process consists of determining static acceleration
   * values due to gravity by placing the axis opposite and in the direction of gravity. This gives
   * a minimum and maximum value. The difference between the two is equal to 2G and used to
   * calculate a scale factor. The mean of the two corresponds to 0G and equals the offset value.
   *
   * <p>Calibration settings are held in memory. To store these values one should use the <code>
   * {@link #save}</code> method.
   *
   * @param axis Axis should have the value X,Y or Z
   */
  public void calibrateAxis(char axis) {
    float max = 0;
    float min = 0;
    float minG = 0;
    float maxG = 0;
    float oldOffset = 0;
    float oldScale = 0;
    int index = 0;
    while (Button.ENTER.isDown()) ;
    switch (axis) {
      case 'X':
        index = 0;
        break;
      case 'Y':
        index = 1;
        break;
      case 'Z':
        index = 2;
        break;
      default:
        return;
    }
    oldOffset = offset[index];
    oldScale = scale[index];
    offset[index] = 0;
    scale[index] = 1;
    LCD.clear();
    LCD.drawString("Point " + axis + " up.", 0, 1);
    LCD.drawString("Press enter to start", 0, 2);
    showLowPass(index);
    LCD.drawString("Sampling...         ", 0, 2);
    Sound.beep();
    max = getRawMean(index);
    maxG = getMeanG(index);
    Sound.beep();

    LCD.clear();
    LCD.drawString("Point " + axis + " down.", 0, 1);
    LCD.drawString("Press enter to start", 0, 2);
    showLowPass(index);
    LCD.drawString("Sampling...         ", 0, 2);
    Sound.beep();
    min = getRawMean(index);
    minG = getMeanG(index);
    Sound.beep();

    LCD.clear();
    LCD.drawString("offset range", 4, 0);
    LCD.drawString("old", 0, 2);
    LCD.drawString(Double.toString(oldOffset), 4, 2);
    LCD.drawString(Double.toString(oldScale), 11, 2);
    LCD.drawString("new", 0, 3);
    offset[index] = (min + max) / 2.f;
    scale[index] = 2.f / (maxG - minG);
    LCD.drawString(Double.toString(offset[index]), 4, 3);
    LCD.drawString(Double.toString(scale[index]), 11, 3);
    LCD.drawString("Press enter", 0, 7);
    Button.ENTER.waitForPressAndRelease();
  }
예제 #3
0
 public static void Finished() {
   // execute all actions of this state
   LCD.drawString("Finished", 0, 3);
   Sound.beep();
   Sound.beep();
   Sound.beep();
   btThread.write(300); // sends the message 'all done'
 }
  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();
  }
예제 #5
0
 public static void main(String[] args) {
   try {
     Thread.setDefaultUncaughtExceptionHandler(new ThreadHandler());
     Sound.playTone(2000, 300);
     BtReceiver btReceiver = new BtReceiver();
     btReceiver.connect();
   } catch (Exception e) {
     Sound.buzz();
     System.out.println("Main: exception");
     System.out.println(e.toString());
     System.out.print(e.getMessage());
     Utils.sleep(1000);
     Button.waitForAnyPress();
     System.exit(0);
   }
 }
예제 #6
0
  public void run() {
    System.out.println("Watching stalls");

    while (!Thread.interrupted()) {
      index = 0;
      for (Motor m : motors) {
        newSpeed = m.getActualSpeed();
        if (m.isRotating()) {
          if (newSpeed < limit) {
            if (oldSpeed[index] > newSpeed || wasStalled[index]) {
              if (oldSpeed[index] > newSpeed) {
                Sound.beep();
              }
              stalled = true;
              m.flt();
              wasStalled[index] = false;
            }
            wasStalled[index] = true;
          }
        } else {
          oldSpeed[index] = 0;
        }
        oldSpeed[index] = newSpeed;
        index++;
        // TODO: this will miss a chunk abut once every 20-50 (actually more, this probably takes
        // at least 10ms)
        try {
          Thread.sleep(95);
        } catch (InterruptedException e) {
        }
      }
    }
  }
예제 #7
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();
  }
예제 #8
0
파일: Init.java 프로젝트: Canardou/ptut
  /**
   * Initialise divers paramètre
   *
   * @see Param#VOLUME
   * @see Param#SPEED_CRUISE
   * @see Param#RSPEED_CRUISE
   * @see Param#RSPEED_SONARMOTOR
   */
  private void init(ThreadRobot tRobot) {
    Sound.setVolume(VOLUME);
    tRobot.getMov().getDiffPilot().setTravelSpeed(Movement.SPEED_CRUISE);
    tRobot.getMov().getDiffPilot().setRotateSpeed(Movement.RSPEED_CRUISE);
    tRobot.getSonarMotor().setSpeed(Movement.RSPEED_SONARMOTOR);

    tRobot.getEnv().setInitValues(10, 10, 0); // A faire depuis la com
  }
  // The recognize() method returns true if the object is a blue block
  public boolean recognize() {

    // Boolean that determines whether or not to start detecting what kind of object is in front
    boolean isRecognizing = true;

    while (isRecognizing) {

      // The robot will drive slowly if the light reads under 300, which means an object is closeby
      while (color.getNormalizedLightValue() < 300) {
        robot.setForwardSpeed(2);
      }

      // When the robot is close enough to the object, it will stop then turn on its
      // blue light which gives two noticeable different light values for a wooden and blue
      // block

      robot.setForwardSpeed(0);
      color.setFloodlight(Color.BLUE);

      // If the light value of blue reads over 250, it is a blue block
      if (color.getNormalizedLightValue() >= 250) {

        Sound.beep();
        LCD.drawString("Blue styrofoam block", 3, 5);

        // Turn the default light back on after analyzing an object
        color.setFloodlight(true);

        return true;

      } else { // If it is not a blue block, it is a wooden block

        Sound.buzz();
        LCD.drawString("Wooden block", 3, 5);

        // Turn the default light back on after analyzing an object
        color.setFloodlight(true);
      }

      // Set the boolean to false to break out of the while loop and stop analyzing the object
      isRecognizing = false;
    }

    // Default return value. It should never get here but JAVA requires a return type
    return false;
  }
예제 #10
0
 public void run() {
   while (true) {
     for (int i = 0; i < note1.length; i += 2) {
       final int tone = (int) note1[i];
       final int b = i + 1;
       final int length = 10 * note1[b];
       Sound.playTone(tone, length);
       try {
         Thread.sleep(length);
       } catch (InterruptedException e) {
         e.printStackTrace();
       }
     }
   }
 }
예제 #11
0
  public static void Sonarflag() {
    // execute all actions of this state
    LCD.drawString("Sonarflag", 0, 3);
    Sound.beep();
    btThread.write(sonar.getDistance()); // sends the data from the sonar

    // leg de huidige tijd vast voor alle transitions met een timeoutcondition
    long starttime = System.currentTimeMillis();

    // when done, wait for a trigger for a transition
    boolean transitionTaken = false;
    while (!transitionTaken) {
      if ((true)) {
        current = State.WATCH;
        transitionTaken = true;
      }
    }
  }
예제 #12
0
  public static void main(String[] args) {

    int option = 0;

    while (option == 0) option = Button.waitForAnyPress();

    MobileRobot robot = new MobileRobot();

    MobileRobot.corr.turnOnLightSensors();
    MobileRobot.corr.startCorrectionTimer();
    robot.liftClaw();

    robot.performRotationCorrectionLocalization();

    // robot.travelCoordinate(0,0,true);

    Sound.beep();
  }
예제 #13
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();
  }
 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;
 }
예제 #15
0
  public void MyTune() {

    // NOTE: This tune was generated from a midi using Guy
    // Truffelli's Brick Music Studio www.aga.it/~guy/lego
    short[] note = {
      2349, 115, 0, 5, 1760, 165, 0, 35, 1760, 28, 0, 13, 1976, 23, 0, 18, 1760, 18, 0, 23, 1568,
      15, 0, 25, 1480, 103, 0, 18, 1175, 180, 0, 20, 1760, 18, 0, 23, 1976, 20, 0, 20, 1760, 15, 0,
      25, 1568, 15, 0, 25, 2217, 98, 0, 23, 1760, 88, 0, 33, 1760, 75, 0, 5, 1760, 20, 0, 20, 1760,
      20, 0, 20, 1976, 18, 0, 23, 1760, 18, 0, 23, 2217, 225, 0, 15, 2217, 218
    };
    for (int i = 0; i < note.length; i += 2) {
      final short w = note[i + 1];
      final int n = note[i];
      if (n != 0) Sound.playTone(n, w * 10);
      try {
        Thread.sleep(w * 2);
      } catch (InterruptedException e) {
      }
    }
  }
예제 #16
0
  public void run() {
    System.out.println("Connecting to " + name);
    conn = Bluetooth.connect(Bluetooth.getKnownDevice(name));
    conn.setIOMode(NXTConnection.PACKET);

    dis = conn.openDataInputStream();
    dos = conn.openDataOutputStream();
    System.out.println("NXT connected");

    // Request dimensions
    try {
      dos.writeByte(3);
      dos.flush();
    } catch (IOException e1) {
    }

    try {
      while (true) {
        byte type = dis.readByte();
        switch (type) {
          case 1:
            byte slot = dis.readByte();
            byte state = dis.readByte();
            states[slot] = state;
            break;

          case 3:
            slotCount = dis.readByte();
            stateCount = dis.readByte();
            break;

          case 127:
            EStop.thread.activate = true;
            break;
        }
      }
    } catch (IOException e) {
      Sound.buzz();
      System.out.println("Comm dropped to " + name);
    }
  }
  public static void main(String... args) throws InterruptedException, IOException {
    Thread display = new SoccerDisplay(robot);
    display.start();
    robot.pilot.setRegulation(true);

    while (true) {
      robot.pilot.setMoveSpeed(50);
      while (!robot.IR.hasDirection()) {
        robot.status = "Searching";
        robot.pilot.travel(180);
        Thread.sleep(100);
      }
      robot.pilot.setMoveSpeed(200);
      robot.pilot.stop();
      while (robot.IR.hasDirection()) {
        while (robot.hasBall()) {
          robot.status = "Dribbling";
          robot.pilot.travel(0);
          Sound.beepSequenceUp();
          Thread.sleep(250);
          robot.kick();
          robot.pilot.stop();
          Thread.sleep(500);
        }

        float angle = robot.IR.getAngle();

        if (Math.abs(angle) > 60) {
          // robot.status = "Behind";
          // float angle2 = 180 - Math.signum(angle) * (180 - Math.abs(angle)) * 0.75f;
          robot.status = "" + angle; // + ", " + angle2 + "        ";
          robot.pilot.travel(angle); // 2);

          Thread.sleep(500);
        } else if (!Float.isNaN(angle)) {
          robot.pilot.travel(angle);
        }
        Thread.sleep(100);
      }
    }
  }
예제 #18
0
  public static void Colorflag() {
    // execute all actions of this state
    LCD.drawString("Colorflag", 0, 3);
    Sound.beep();
    btThread.write(400); // sends the message 'new color found'

    // leg de huidige tijd vast voor alle transitions met een timeoutcondition
    long starttime = System.currentTimeMillis();

    // when done, wait for a trigger for a transition
    boolean transitionTaken = false;
    while (!transitionTaken) {
      if ((starttime + 30000 <= System.currentTimeMillis())) {
        current = State.WATCH;
        transitionTaken = true;
      } else if ((btThread.peekElement() == 500)) {
        current = State.MEASUREMENT;
        transitionTaken = true;
      }
    }
  }
 /**
  * Loads saved offset and scale values from flash. If no saved values are found it will load
  * default values for offset=0 and scale =1;
  */
 public void load() {
   File store = new File(name);
   FileInputStream in = null;
   if (store.exists()) {
     try {
       in = new FileInputStream(store);
       DataInputStream din = new DataInputStream(in);
       for (int i = 0; i < 3; i++) {
         offset[i] = din.readFloat();
         scale[i] = din.readFloat();
       }
       din.close();
     } catch (IOException e) {
       System.err.println("Failed to load calibration");
       Sound.beep();
     }
   } else {
     for (int i = 0; i < 3; i++) {
       offset[i] = 0;
       scale[i] = 1;
     }
   }
 }
예제 #20
0
파일: Init.java 프로젝트: Canardou/ptut
  /** Fonction d'initialisation test */
  public static void initTest(ThreadRobot tRobot) {
    Sound.setVolume(VOLUME);
    tRobot.getMov().getDiffPilot().setTravelSpeed(Movement.SPEED_CRUISE);
    tRobot.getMov().getDiffPilot().setRotateSpeed(Movement.RSPEED_CRUISE);
    tRobot.getSonarMotor().setSpeed(Movement.RSPEED_SONARMOTOR);

    tRobot.getEnv().setInitValues(10, 10, 0);

    tRobot.getOrder().add(Order.WAITBUTTON);
    tRobot.getOrder().chooseInsecurely();
    tRobot.getOrder().execute();
    tRobot.getOrder().add(Order.WAIT1SEC);
    tRobot.getOrder().chooseInsecurely();
    tRobot.getOrder().execute();
    tRobot.getOrder().add(Order.SETPOSITION);
    tRobot.getOrder().chooseInsecurely();
    tRobot.getOrder().execute();
    tRobot.getOrder().add(Order.SAVEREFANGLE);
    tRobot.getOrder().chooseInsecurely();
    tRobot.getOrder().execute();
    tRobot.getOrder().add(Order.CHECKFIRSTCASE);
    tRobot.getOrder().chooseInsecurely();
    tRobot.getOrder().execute();
  }
예제 #21
0
  /** Runs the ai... */
  public void run() {
    Communicator comm = null;
    try {
      comm = Communicator.getInstance(this);
    } catch (Exception e) {
      Console.println("-------------------");
      Console.println("ERROR in Comm");
      Console.println("-------------------");
      Button.waitForPress();
      return;
    }
    while (grid.isWorkToDo()) {
      PathElement p = null;
      if (motion.isObjLoaded()) {
        p = grid.getAWayBackHome(robo);
      } else if ((p = grid.getAWayToNextKnownUnCommon(robo)) == null) {
        if ((p = grid.getAWayToNextUnknown(robo)) == null) {
          /* does not work..
          if ((p = grid.getAWayToNextKnownCommon(robo)) == null) {
          	try {
          		sleep(500);
          	} catch (InterruptedException e) {
          		// can't sleep, so do a busy waiting
          	}
          	continue;	// try again
          }*/
        }
      }
      Console.println("here3"); // TODO
      do {
        orient(p);

        // ask other
        if (!askOther(p)) continue; // try later again

        // if its not for me, i won't go there. (see grid.getway...)
        if (grid.getJunction(p).getType() == Junction.MASTER_OBJ
            || grid.getJunction(p).getType() == Junction.SLAVE_OBJ) {
          // load
          motion.goToNextJunction(true);
          grid.setJunction(new Junction(robo.getMyActualPosition(), Junction.EMPTY), true);
        } else {
          // unloading loaded object
          if (motion.isObjLoaded()
              && grid.getJunction(grid.getNextProjectedPosition(robo)).getType()
                  == Junction.HOME_BASE) {

            motion.goToNextJunction(false);

            // orient to south
            Position orientation = grid.getNextProjectedPosition(robo);
            Position unloadPos =
                new Position(robo.getHomeBase().getX(), robo.getHomeBase().getY() - 1);

            // unload the object
            orient(unloadPos);
            motion.unload();

            // orient back
            orient(orientation);

            // regular move
          } else motion.goToNextJunction(false);
        }

      } while ((p = p.getNextElt()) != null);
    }
    Console.println(" - FIN - ");
    Sound.playTone(400, 1000);
    Button.waitForPress();
  }
예제 #22
0
파일: Sound.java 프로젝트: lego-line/lejos
 /**
  * Plays a sound file once from the NXT. SoundSensor files use the .rso extension. The filename is
  * not case sensitive. Filenames on the NXT Bricks display do now show the filename extension.
  *
  * @param fileName e.g. "Woops.rso"
  * @return If you receive a non-zero number, the filename is probably wrong or the file is not
  *     uploaded to the NXT brick.
  */
 public static byte playSoundFile(String fileName) {
   return Sound.playSoundFile(fileName, false);
 }
예제 #23
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();
      }
    }
  }
예제 #24
0
  /**
   * Localizes the robot using the ultrasonic sensor. Clocks the angle at which the sensor detects
   * each wall and calculates 0 degrees
   */
  public void doLocalization() {

    angleA = 0;
    angleB = 0;
    int UCdistance, expmtDistance;
    expmtDistance = 38;
    int count = 0;
    boolean facingToWall = true;

    // rotate the robot until it sees no wall
    nav.rotate(165, -165);
    facingToWall = true;

    while (facingToWall) {
      UCdistance = getFilteredData();
      if (UCdistance > expmtDistance) {
        count++;
      }
      if (UCdistance > expmtDistance && count >= 3) {
        facingToWall = false;
        count = 0;
      }
    }

    sleep(3000);

    // keep rotating until the robot sees a wall, then latch the angle
    while (!facingToWall) {
      UCdistance = getFilteredData();
      if (UCdistance < expmtDistance) {
        count++;
      }

      if (UCdistance < expmtDistance && count >= 3) {
        facingToWall = true;
        angleA = odo.getTheta();
        Sound.beep();
        nav.stopMotor();
        count = 0;
      }
    }

    // switch direction and wait until it sees no wall
    nav.rotate(-165, 165);
    while (facingToWall) {
      UCdistance = getFilteredData();

      if (UCdistance > expmtDistance) {
        count++;
      }

      if (UCdistance > expmtDistance && count >= 3) {
        facingToWall = false;
        count = 0;
      }
    }

    // keep rotating until the robot sees a wall, then latch the angle
    sleep(3000);

    while (!facingToWall) {
      UCdistance = getFilteredData();
      if (UCdistance < expmtDistance) {
        count++;
      }

      if (UCdistance < expmtDistance && count >= 3) {
        facingToWall = true;
        angleB = odo.getTheta();
        Sound.beep();
        nav.stopMotor();
      }
    }

    // angleA is clockwise from angleB, so assume the average of the
    // angles to the right of angleB is 45 degrees past 'north'
    // we slightly correct the angle '45' to '44' based on the error we measured
    angle = 45 - (angleA - angleB) / 2;

    // update the odometer position (example to follow:)
    odo.setTheta(angle);
    nav.turnTo(0);
  }