Programando robo

[biza 1]

Malta estou programado um robô e gostaria de chamar uma função de tempo em tempo que verifica-se a distancia do sensor a parede lateral direita o código que desenvolvi é o seguinte:

import lejos.nxt.*;
/**************************************************************************************************
*																								  *
*								SIMULADOR DE ANTI-QUEDAS MESA									  *
*																								  *
***************************************************************************************************/					
public class labirinto{
//declarar variaveis
static boolean quit = false;
static int SPEED, NEG_TURN, POS_TURN, SENSOR_DELAY, FRONT_SENSOR, RIGHT_SENSOR, FRONTAL_DIST, LATERAL_DIST, estados, TEMPORIZADOR;

	public static void main(String [] args) throws InterruptedException {
	
		SENSOR_DELAY = 100; /*CHAMADAS DO SENSOR DIREIRO A PAREDE*/
		TEMPORIZADOR = 500; /*TEMPORIZADOR DE INSTRUCAO*/
		SPEED = 900; /*VELOCIDADE DOS MOTORES DO CARRO*/
		NEG_TURN = -45; /*ANGULO DE VIRAGEM*/
		POS_TURN = 45; /*ANGULO DE VIRAGEM*/
		FRONTAL_DIST = 15;  /*DISTANCIA DO SENSOR FRONTAL AO OBJECTO*/
		LATERAL_DIST = 25; /*DISTANCIA DO SENSOR LATERAL AO OBJECTO*/
		
		/*SENSOR DE ULTRASONS DETETA ATE 170CM*/
		UltrasonicSensor  ultra_frontal = new UltrasonicSensor(SensorPort.S2); /*SENSOR DE ULTRASONS FRONTAL*/
		UltrasonicSensor  ultra_lateral = new UltrasonicSensor(SensorPort.S3); /*SENSOR DE ULTRASONS LATERAL*/
	    ultra_lateral.continuous();
		estados=0;
		LCD.drawString("LIGAR MOTOR", 0, 2);
		Thread.sleep(TEMPORIZADOR);
		
		/*ENQUANTO O CICLO WHILE NAO FOR INTERROMPIDO DENTRO DESTE OPERA EM CONTINUO*/	
		while(!quit){
		
		
		FRONT_SENSOR = ultra_frontal.getDistance(); /*DEVOLVE A DISTANCIA AO OBJECTO FRONTAL*/ 
		RIGHT_SENSOR = ultra_lateral .getDistance(); /*VOLVE A DISTANCIA AO OBJECTO LATERAL DIREITO*/
		
		LCD.drawInt((int)(RIGHT_SENSOR),3,3); 
		
			/*CASO A DISTANCIA LIDA PELO SENSOR SEJA SUPERIOR A DISTANCIA ADMITIDA*/
			if(FRONT_SENSOR > FRONTAL_DIST){
			estados = 0; /*CARRO ANDA EM FRENTE*/ 
			}else if(FRONT_SENSOR < FRONTAL_DIST && RIGHT_SENSOR < LATERAL_DIST){
			estados = 1;/*CARRO VIRA NO SENTIDO ANTI-HORARIO <---------*/
			}else if(FRONT_SENSOR < FRONTAL_DIST && RIGHT_SENSOR > LATERAL_DIST){
			/*CARRO VIRA NO SENTIDO HORARIO ---------->*/
			estados = 2;
			}
			
			/****MAQUINA DE ESTADOS****/
			switch(estados){
			/*ANDA EM FRENTE ATE DETECTAR OBSTACULO*/
			case 0:
			straight();
			break;
			/*VIRA NO SENTIDO ANTI-HORARIO*/
			case 1:
			turn_left();
			break;
			/*VIRA NO SENTIDO ANTI-HORARIO*/
			case 2:
			turn_right();
			break;
			}
			/*SAIR DO APLICATIVO*/
			if(Button.ESCAPE.isPressed()){
				LCD.clear(); 
				LCD.drawString("A DESLIGAR",4,4);
				Thread.sleep(TEMPORIZADOR);
				quit=true;
			}

		}/*END WHILE*/

	}/*END MAIN*/
	
	/*MEDICOES A PAREDE*/
	public static void lateral_interrupt( ){
		LCD.clear(); 
		LCD.refresh();
		LCD.drawString("WALL-",0,6);
		LCD.drawInt((int)(RIGHT_SENSOR),8,6);
		try{Thread.sleep(SENSOR_DELAY);}catch (InterruptedException e){}
	}
	/*VIRAR A ESQUERDA*/
	public static void turn_left(){
		LCD.clear(); 
		LCD.refresh();
		LCD.drawString("-90 GRAUS",6,4);
		Motor.C.backward(); /*MOTOR [A] PARA TRAS*/
		Motor.C.rotate(NEG_TURN);
		Motor.A.forward(); /*MOTOR [c] ANDA PARA A FRENTE*/
		Motor.A.rotate(POS_TURN);
		try{Thread.sleep(SENSOR_DELAY);}catch (InterruptedException e){}
	}
	/*VIRAR A DIREITA*/
	public static void turn_right(){
		LCD.clear(); 
		LCD.refresh();
		LCD.drawString("90 GRAUS",6,4);
		Motor.A.backward(); /*MOTOR [A] PARA */
		Motor.A.rotate(NEG_TURN);
		Motor.C.forward(); /*MOTOR [c] ANDA PARA A FRENTE*/
		Motor.C.rotate(POS_TURN);
		try{Thread.sleep(SENSOR_DELAY);}catch (InterruptedException e){}
	}
	
	/*ANDAR EM FRENTE*/
	public static void straight(){
		RIGHT_SENSOR = ultra_lateral .getDistance(); 
		LCD.clear(); 
		LCD.refresh();
		LCD.drawString("EM FRENTRE",6,4);
		Motor.A.forward(); /*MOTOR [A] ANDA PARA A FRENTE */
		Motor.A.setSpeed(SPEED);
		if( RIGHT_SENSOR > LATERAL_DIST){
		Motor.C.forward(); /*MOTOR [C] ANDA PARA A FRENTE */
		Motor.C.setSpeed(SPEED-20);
		}else{
		Motor.C.forward(); /*MOTOR [C] ANDA PARA A FRENTE */
		Motor.C.setSpeed(SPEED);
		}
		try{Thread.sleep(TEMPORIZADOR);}catch (InterruptedException e){}
	}
	/*CORRECCAO DA TRAGECTORIA*/
	public static void correction (){

	}
	
}/*END CLASS MESA*/

O Código esta totalmente funcional a excepção que não faz as devidas leituras a parede