Laberinto

import lejos.nxt.*;
import java.io.*;

public class Labyrinth1 {
 
 //CONSTANTS
 private static int forwardStep = 720;
 private static int turnStep = 410;
 private static int fdist = 15;
 private static int slack = 20;
 private static int turnSpeed = 200;
 private static int straightSpeed = 400;
 
 //VARIABLES
 private static Motor rightMotor = Motor.B;
 private static Motor leftMotor = Motor.C;
 private static TouchSensor leftSensor = new TouchSensor(SensorPort.S2);
 private static TouchSensor rightSensor = new TouchSensor(SensorPort.S1);
 private static PrintStream console = new PrintStream(new LCDOutputStream());
 private static UltrasonicSensor sens = new UltrasonicSensor(SensorPort.S4);
 
 public static void main (String[] aArg) {

  //right hand rule
  while(true){
   try{
    while(moveOneSquare() == false)
     //do nothing
     ;
    
    rightMotor.setSpeed(turnSpeed);
    leftMotor.setSpeed(turnSpeed);
    
    console.println("Looking Right");
    //prepara a girar
    rightSlack();
    turnRight();
    //pone en neutral
    leftSlack();
    if (sens.getDistance()<fdist){
     console.println("Looking Front");
     //prepara a girar
     leftSlack();
     turnLeft();
     if(sens.getDistance()<fdist){
      console.println("Looking Left");
      turnLeft();
      if(sens.getDistance()<fdist){
       console.println("Going Back");
       turnLeft();
      }
     }
    }
    //pone en neutral
    rightSlack();
    
    if (leftSensor.isPressed() || rightSensor.isPressed())
     panic();
    
    //Stop wobbling
    Thread.sleep(200);
   }
   catch(Exception ex) {
    console.println("Something happened.\nI dont care.");
   }
  }
 }
 
 private static boolean moveOneSquare() throws Exception{
  boolean left, right;
  rightMotor.setSpeed(straightSpeed);
  leftMotor.setSpeed(straightSpeed);
  rightMotor.resetTachoCount();
  leftMotor.resetTachoCount();
  
  leftMotor.rotate(forwardStep,true);
  rightMotor.rotate(forwardStep, true);
  
  while(leftMotor.isRotating() && rightMotor.isRotating()){
   left = leftSensor.isPressed();
   right = rightSensor.isPressed();
   if ( left || right ){
    leftMotor.stop();
    rightMotor.stop();
    leftMotor.rotate(-leftMotor.getTachoCount(),true);
    rightMotor.rotate(-rightMotor.getTachoCount(), false);
    if (left && right){
     panic();
    }
    //left
    else if(left){
     rightMotor.rotate(-turnStep/5);
    }
    //right
    else {
     leftMotor.rotate(-turnStep/5);
    }  
    //try again!
    return false;
   }
  }
  //success!!
  return true;
 }
 
 //tensar la cuerda (== los engranages)
 //para que un giro a la derecha empieza de inmediato
 private static void rightSlack() throws Exception{
  rightMotor.rotate(-slack,true);
  leftMotor.rotate(slack,false);
  Thread.sleep(200);
 }
 //tensar la cuerda (== los engranages)
 //para que un giro a la izquierda empieza de inmediato
 private static void leftSlack() throws Exception {
  leftMotor.rotate(-slack,true);
  rightMotor.rotate(slack,false);
  Thread.sleep(200);
 }
 private static void turnRight() throws Exception {
  rightMotor.rotate(-(turnStep-30),true);
  leftMotor.rotate(turnStep-30,false);
  Thread.sleep(200);
 }
 private static void turnLeft() throws Exception {
  leftMotor.rotate(-turnStep,true);
  rightMotor.rotate(turnStep,false);
  Thread.sleep(200);
 }
 private static void panic() throws Exception {
  console.println("!!! HELP !!!");
  console.println("Put me straight and center");
  console.println("and press Enter.");
  Sound.beepSequenceUp();
  Button.ENTER.waitForPressAndRelease();
 }
}

Otra fuente: http://agentesotono2010.blogspot.mx/2010/11/mindstorms-nxt-lejos-salir-de-un.html