Robot: New page: import lejos.navigation.SimpleNavigator; import lejos.nxt.; public class GoalFinder1 { / This program will let the robot follow the black line until it detects ...

2009-08-13T16:19:56Z

New page: <source lang="java"> import lejos.navigation.SimpleNavigator; import lejos.nxt.*; public class GoalFinder1 { /* This program will let the robot follow the black line until it detects ...

New page

<source lang="java">
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;

}

}

</source>

NJM Source Code - Revision history

Robot: New page: import lejos.navigation.SimpleNavigator; import lejos.nxt.*; public class GoalFinder1 { /* This program will let the robot follow the black line until it detects ...

Robot: New page: import lejos.navigation.SimpleNavigator; import lejos.nxt.; public class GoalFinder1 { / This program will let the robot follow the black line until it detects ...