NJM Source Code

import lejos.navigation.SimpleNavigator;
import lejos.nxt.*;


public class GoalFinder1 {

	/* This program will let the robot follow the black line until
	it detects a crossing. At a crossing the robot always takes the
	rightmost turn. This procedure will allow the robot the find a
	goal at the end of the maze. */
	
	
	// initializing the navigator
	static SimpleNavigator navigator = new SimpleNavigator(5.6f,11.5f,Motor.C,Motor.B);
	static int threshold = 50;
		
	// main program
	
	public static void main(String[] args) throws InterruptedException {
		
		
		// initializing variables
		int valueLightCornerRight, valueLightCornerLeft, valueLightFollow;
		int i = 1;
						
		// initializing sensors and navigator speed
		LightSensor lightCornerRight = new LightSensor(SensorPort.S2);
		LightSensor lightCornerLeft = new LightSensor(SensorPort.S4);
		LightSensor lightFollow = new LightSensor(SensorPort.S3);
		navigator.setSpeed(180);
		
		Thread.sleep(1000);
		
		while(true){
					
				// read the sensor values and print them on the screen
				valueLightCornerRight = lightCornerRight.readValue();
				valueLightCornerLeft = lightCornerLeft.readValue();
				valueLightFollow = lightFollow.readValue();
								
				LCD.drawInt(valueLightCornerRight, 0, 0);
				LCD.drawString("corner right", 4, 0);
				LCD.drawInt(valueLightCornerLeft, 0, 1);
				LCD.drawString("corner left", 4, 1);
				LCD.drawInt(valueLightFollow, 0, 2);
				LCD.drawString("middle", 4, 2);
							
				// follow the line as long as no corner or crossing is detected	
				if(i==1) {
					followLine(valueLightFollow);
					LCD.drawString("following line", 0, 5);
					if(cornerDetected(valueLightCornerRight,valueLightCornerLeft,valueLightFollow)) i++;
					}
													
				// drive forward a little bit when crossing is detected
				if(i==2){
					navigator.travel(9f);
					i++;
					LCD.clear();
					LCD.drawString("drive forward", 0, 5);
					Sound.beep();
				}
				
				// turn right
				if(i==3){
					boolean line = false;
					navigator.rotate(-140f, true);
					Thread.sleep(1000);
					while(navigator.isMoving()){
							valueLightCornerRight = lightCornerRight.readValue();
							valueLightCornerLeft = lightCornerLeft.readValue();
							valueLightFollow = lightFollow.readValue();
							LCD.drawInt(valueLightCornerRight, 0, 0);
							LCD.drawString("corner right", 4, 0);
							LCD.drawInt(valueLightCornerLeft, 0, 1);
							LCD.drawString("corner left", 4, 1);
							LCD.drawInt(valueLightFollow, 0, 2);
							LCD.drawString("middle", 4, 2);
							if(valueLightCornerLeft<threshold)	{
								i = 1;
								line = true;
								navigator.stop();
								Sound.beep();
								break;
							}
						}
					if(!line) i++;
				}
				
				// forward
				if(i==4){
					boolean line = false;
					navigator.rotate(150f, true);
					Thread.sleep(1000);
					while(navigator.isMoving()){
							valueLightCornerRight = lightCornerRight.readValue();
							valueLightCornerLeft = lightCornerLeft.readValue();
							valueLightFollow = lightFollow.readValue();
							LCD.drawInt(valueLightCornerRight, 0, 0);
							LCD.drawString("corner right", 4, 0);
							LCD.drawInt(valueLightCornerLeft, 0, 1);
							LCD.drawString("corner left", 4, 1);
							LCD.drawInt(valueLightFollow, 0, 2);
							LCD.drawString("middle", 4, 2);
							if( valueLightFollow<threshold)	{
								i = 1;
								line = true;
								navigator.stop();
								Sound.beep();
								break;
							}
						}
					if(!line) i++;
				}
				
				// turn left
				if(i==5){
					boolean line = false;
					navigator.rotate(110f, true);
					Thread.sleep(1000);
					while(navigator.isMoving()){
							valueLightCornerRight = lightCornerRight.readValue();
							valueLightCornerLeft = lightCornerLeft.readValue();
							valueLightFollow = lightFollow.readValue();
							LCD.drawInt(valueLightCornerRight, 0, 0);
							LCD.drawString("corner right", 4, 0);
							LCD.drawInt(valueLightCornerLeft, 0, 1);
							LCD.drawString("corner left", 4, 1);
							LCD.drawInt(valueLightFollow, 0, 2);
							LCD.drawString("middle", 4, 2);
							if(valueLightFollow<threshold)	{
								i = 1;
								line = true;
								navigator.stop();
								Sound.beep();
								break;
							}
						}
				}
				
			}


	}
	
	
	// following the black line
	
	public static void followLine(int light){
				
			if(light>threshold)
			{
				Motor.B.setSpeed(300);
				Motor.B.forward();
				Motor.C.setSpeed(150);
				Motor.C.backward();
			}
			
			else 
			{
				Motor.C.setSpeed(300);
				Motor.C.forward();
				Motor.B.setSpeed(150);
				Motor.B.backward();
			}
		
		}
	
	
	// detecting corner or crossing
	
	public static boolean cornerDetected(int lightRight, int lightLeft, int lightCenter){
						
			if((lightLeft<40 || lightRight<40) && lightCenter<threshold)	return true;
			else return false;
			
		}

}