NXTUtils.java

import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;

public class NXTUtils {

	public static int getAvgUltrasonicSensorReading () {
		
		int sumUpDistances = 0;
		int avgDistance = 0;
		
		UltrasonicSensor ultrasonicSensor = new UltrasonicSensor(SensorPort.S1);
		
		for (int i = 0; i < 5; ++i) {
			ultrasonicSensor.continuous();
			try {
				Thread.sleep(25);
			} catch (InterruptedException e) {

			}
			int distance = ultrasonicSensor.getDistance();
			sumUpDistances += distance;
		}
		
		avgDistance = sumUpDistances / 5;

		try {
			Thread.sleep(100);
		} catch (InterruptedException e) {

		}

		return avgDistance;
	}
	
}

OmniBiped.java

import lejos.nxt.Button;
import lejos.nxt.Motor;

public class OmniBiped {

	private static final int WALK_SPEED = 540; //270
	private static final int TURN_SPEED = 630; //360
	private static final int NEAR = 24; 
	private static final int FAR = 50;
	private static final int FULL_STEP = 1080;
	private static final long HALF_STEP = 540;
	private static final int ALIGN_SPEED = 450;  //180

	public static void main(String[] args) {
		int direction;

		init();

		while (Button.ESCAPE.isUp()) {

			Motor.A.setSpeed(WALK_SPEED);
			Motor.B.setSpeed(WALK_SPEED);
			Motor.A.forward();
			Motor.B.forward();

			while (NXTUtils.getAvgUltrasonicSensorReading() > NEAR && Button.ESCAPE.isUp())
				;

			Motor.A.stop();
			Motor.B.stop();

			realignLegs();

			if (NXTUtils.getAvgUltrasonicSensorReading() < FAR) {
//				Motor.A.setSpeed(WALK_SPEED);
//				Motor.B.setSpeed(WALK_SPEED);
//				Motor.A.backward();
//				Motor.B.backward();
//
//				while (NXTUtils.getAvgUltrasonicSensorReading() < FAR)
//					;
//
//				Motor.A.stop();
//				Motor.B.stop();
//
//				realignLegs();

				direction = 1 - 2 * ((int) Math.random() * 2);

				if (direction == 1 || direction == 0) {
					// Motor.A.rotate(+90);
					// Motor.B.rotate(-90);
					Motor.A.setSpeed(TURN_SPEED);
					Motor.B.setSpeed(TURN_SPEED);
					Motor.A.backward();
					Motor.B.forward();
				} else if (direction == -1) {
					// Motor.A.rotate(-90);
					// Motor.B.rotate(+90);
					Motor.A.setSpeed(TURN_SPEED);
					Motor.B.setSpeed(TURN_SPEED);
					Motor.A.forward();
					Motor.B.backward();
				}
				
				while (NXTUtils.getAvgUltrasonicSensorReading() < FAR && Button.ESCAPE.isUp());
				
				Motor.A.stop();
				Motor.B.stop();
				
				realignLegs();
			}

		}
	}

	private static void realignLegs() {
		// TODO Auto-generated method stub
		long right_count, left_count;

		right_count = Math.abs(Motor.B.getTachoCount() % FULL_STEP);
		left_count = Math.abs(Motor.A.getTachoCount() % FULL_STEP);

		if (right_count < HALF_STEP) {
			Motor.B.setSpeed(ALIGN_SPEED);
			Motor.B.backward();
		} else {
			Motor.B.setSpeed(ALIGN_SPEED);
			Motor.B.forward();
		}

		while (right_count > 0) {
			right_count = Math.abs(Motor.B.getTachoCount() % FULL_STEP);
		}

		Motor.B.stop();

		if (left_count < HALF_STEP) {
			Motor.A.setSpeed(ALIGN_SPEED);
			Motor.A.backward();
		} else {
			Motor.A.setSpeed(ALIGN_SPEED);
			Motor.A.forward();
		}

		while (left_count > 0) {
			left_count = Math.abs(Motor.A.getTachoCount() % FULL_STEP);
		}

		Motor.A.stop();
	}

	private static void init() {
		// TODO Auto-generated method stub
		Motor.A.resetTachoCount();
		Motor.B.resetTachoCount();

		realignLegs();
	}
}