LineFollower.java
import java.io.EOFException;
import java.util.Date;
 
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.addon.RCXLightSensor;
 
public class LineFollower {
    private static RCXLightSensor leftSensor = new RCXLightSensor(SensorPort.S4);
    private static RCXLightSensor rightSensor = new RCXLightSensor(SensorPort.S1);
    private static final int forward = 2;
    private static final int backward = 1;
    private static final int stop = 3;
    private static final int flt = 4;
    private static final int power = 50;
    private static final int minPower = 20;
    private static final int angleForTurning = -345;
    private static int blackWhiteThresholdR, blackWhiteThresholdL, greenWhiteThresholdR, greenWhiteThresholdL;
 
    public static void main (String[] aArg) {
 
        // Use the light sensor as a reflection sensor
        leftSensor.activate();
        rightSensor.activate();
 
        //LCD.drawString("Light %: ", 0, 0);
 
        //DREJER IKKE SKARPT NOK
        try {
        LCD.drawString("Press enter to start", 0, 0);
//        while (! Button.ENTER.isPressed()) {
//            Thread.sleep(10);    
//        }
        int f = 256, d = 500, v = 100; 
        LCD.drawString("Start program", 0, 0);
        calibrate();
        Thread.sleep(1000);    
 
        //Get out of green zone
        driveByTacho(90);                 
        Sound.playTone(f, d, v);
 
        //Drive up the slope
        followStraighLine((int) 5*360, 90, 50); 
        Sound.playTone(f, d, v);
 
        //Reach corner 1
        followStraighLine(999999, 50, 0); 
        Sound.playTone(f, d, v);
 
        //Turn in corner 1
        turnRight(); 
        Sound.playTone(f, d, v);
 
        //Get out of corner 1
        driveByTacho(90); 
        Sound.playTone(f, d, v);
 
        //Reach 2nd slope
        followStraighLine((int)1.5*360, 50, 20); 
        Sound.playTone(f, d, v);
 
        //Drive on 2nd slope
        followStraighLine((int)4.8*360, 90, 50); 
        Sound.playTone(f, d, v);
 
        //Reach 2nd corner
        followStraighLine(999999, 60, 10); 
        Sound.playTone(f, d, v);
 
        //turn in 2nd corner
        turnLeft(); 
        Sound.playTone(f, d, v);
 
        //Get out of 2nd corner
        driveByTacho(90); 
        Sound.playTone(f, d, v);
 
        //Reach 3rd slope
        followStraighLine((int)1.5*360, 50, 20); 
        Sound.playTone(f, d, v);
 
        //Drive 3rd slope
        followStraighLine((int)4.5*360, 90, 50); 
        Sound.playTone(f, d, v);
 
        //Drive into top zone        
        driveByTacho(360); 
        Sound.playTone(f, d, v);
 
        //Turn around
        turnOnTheSpot();
        Sound.playTone(f, d, v);
 
        //Get out of top zone
        driveByTacho(720); 
        Sound.playTone(f, d, v);
 
        //Drive on 3rd slope
        followStraighLine((int)4.5*360, 80, 40); 
        Sound.playTone(f, d, v);
 
        //Reach corner 2
        followStraighLine(999999, 50, 0); 
        Sound.playTone(f, d, v);
 
        //Turn in corner 2
        turnRight(); 
        Sound.playTone(f, d, v);
 
        //Get out of corner 2
        driveByTacho(90); 
        Sound.playTone(f, d, v);
 
        //Reach 2nd slope
        followStraighLine((int)1.5*360, 50, 20); 
        Sound.playTone(f, d, v);
 
        //Drive on 2nd slope
        followStraighLine((int)4.5*360, 90, 50); 
        Sound.playTone(f, d, v);
 
        //Reach 1st corner
        followStraighLine(999999, 50, 0); 
        Sound.playTone(f, d, v);
 
        //turn in 1st corner
        turnLeft(); 
        Sound.playTone(f, d, v);
 
        //Get out of 1st corner
        driveByTacho(90); 
        Sound.playTone(f, d, v);
 
        //Reach 1st slope
        followStraighLine((int)1.5*360, 50, 20); 
        Sound.playTone(f, d, v);
 
        //Drive on 1st slope
        followStraighLine((int)4.5*360, 90, 50); 
        Sound.playTone(f, d, v);
 
        //Drive into green zone
        driveByTacho(360); 
        Sound.playTone(f, d, v);
 
        // Stop car gently with free wheel drive
        MotorPort.B.controlMotor(0,flt);
        MotorPort.C.controlMotor(0,flt);
        LCD.clear();
        LCD.drawString("Program stopped", 0, 0);
        Thread.sleep(1000);
        } catch (Exception e) {
 
        }
    }
 
    private static void driveByTacho(int degrees) {
        Motor.B.resetTachoCount();
        Motor.C.resetTachoCount();
        Motor.B.rotate(-degrees, true);
        Motor.C.rotate(-degrees);
        Motor.B.suspendRegulation();
        Motor.C.suspendRegulation();
    }
 
    private static void calibrate() throws InterruptedException {
        LCD.clear();
        LCD.drawString("Calibrate", 0, 0);
 
        Thread.sleep(1000);
        int blackLightR = 0, blackLightL = 0, whiteLightR = 0, whiteLightL = 0, greenLightR = 0, greenLightL = 0;
        // read black on both sensors
        LCD.drawString("Read black", 0, 1);
        while (!Button.ENTER.isPressed()) {
            blackLightR = 1023 - SensorPort.S4.readRawValue();
            blackLightL = 1023 - SensorPort.S1.readRawValue();
            LCD.drawString("L: " + blackLightL + ", R: " + blackLightR, 0, 2);
        }
 
        Thread.sleep(1000);
        // read white on both sensors
        LCD.drawString("Read white", 0, 1);
        while (!Button.ENTER.isPressed()) {
            whiteLightR = 1023 - SensorPort.S4.readRawValue();
            whiteLightL = 1023 - SensorPort.S1.readRawValue();
            LCD.drawString("L: " + whiteLightL + ", R: " + whiteLightR, 0, 2);
        }
 
        Thread.sleep(1000);
        // read green on both sensors
        LCD.drawString("Read green", 0, 1);
        while (!Button.ENTER.isPressed()) {
            greenLightR = 1023 - SensorPort.S4.readRawValue();
            greenLightL = 1023 - SensorPort.S1.readRawValue();
            LCD.drawString("L: " + greenLightL + ", R: " + greenLightR, 0, 2);
        }
 
        Thread.sleep(1000);
        blackWhiteThresholdL = (3*blackLightL/8 + 5*whiteLightL/8);
        blackWhiteThresholdR = (3*blackLightR/8 + 5*whiteLightR/8);
        greenWhiteThresholdL = (greenLightL/8 + 5*whiteLightL/8);
        greenWhiteThresholdR = (greenLightR/8 + 5*whiteLightR/8);
 
        LCD.clear();
        while (!Button.ENTER.isPressed()) {
            LCD.drawString("bwl: " + blackWhiteThresholdL, 0, 0);
            LCD.drawString("bwr: " + blackWhiteThresholdR, 0, 1);
            LCD.drawString("gwl: " + greenWhiteThresholdL, 0, 2);
            LCD.drawString("gwr: " + greenWhiteThresholdR, 0, 3);
        }
        LCD.clear();
    }
 
    public static void followStraighLine(int targetTacho, int power, int minPower) throws Exception{
        LCD.drawString("Follow straight line", 0, 0);
 
        int tacho = 0;
        Motor.B.resetTachoCount();
        Motor.C.resetTachoCount();
 
        while (! Button.ESCAPE.isPressed() && tacho <= targetTacho) {
 
            tacho = (-1)*(Motor.B.getTachoCount() + Motor.C.getTachoCount())/2;
            Motor.B.suspendRegulation();
            Motor.C.suspendRegulation();
            LCD.drawString("Tacho " + tacho + " - target: " + targetTacho, 0, 7);
 
            int leftLight = 1023 - SensorPort.S4.readRawValue();
            int rightLight = 1023 - SensorPort.S1.readRawValue();
            LCD.drawString("Leftlight  " + leftLight, 0, 3);
            LCD.drawString("Rightlight " + rightLight, 0, 4);
            LCD.drawString("bwl: " + blackWhiteThresholdL, 0, 5);
            LCD.drawString("bwr: " + blackWhiteThresholdR, 0, 6);
 
            if (rightLight < blackWhiteThresholdR && leftLight < blackWhiteThresholdL){
                // Both on black, terminate
                LCD.drawString("Break    ", 0, 2);                
                return;
            } else if (leftLight < blackWhiteThresholdL){
                // Leftlight on black, turn right
                LCD.drawString("Turn right   ", 0, 2);
                MotorPort.B.controlMotor(minPower,forward);
                MotorPort.C.controlMotor(power, forward);
            } else if (rightLight < blackWhiteThresholdR){
                // Rightlight on black, turn left
                LCD.drawString("Turn left  ", 0, 2);
                MotorPort.B.controlMotor(power, forward);
                MotorPort.C.controlMotor(minPower, forward);
            } else if (rightLight > blackWhiteThresholdR && leftLight > blackWhiteThresholdL) {
                // On white, go straight
                LCD.drawString("Go straight ", 0, 2);                
                MotorPort.B.controlMotor(power, forward);
                MotorPort.C.controlMotor(power, forward);
            }
 
            Thread.sleep(5);
        }
        if (Button.ESCAPE.isPressed()) {
            throw new EOFException();
        }
 
    }
 
    public static void turnRight() {
        LCD.drawString("Turn right   ", 0, 0);
        MotorPort.B.controlMotor(0,forward);
        Motor.C.resetTachoCount();
        Motor.C.rotate(angleForTurning);
        Motor.C.suspendRegulation();
        return;
    }
 
    public static void turnLeft() {
        LCD.drawString("Turn left  ", 0, 0);
        MotorPort.C.controlMotor(0,forward);
        Motor.B.resetTachoCount();
        Motor.B.rotate(angleForTurning);
        Motor.B.suspendRegulation();
        return;    
    }
 
    public static void turnOnTheSpot() {
        LCD.drawString("Turn around  ", 0, 0);
        Motor.B.resetTachoCount();
        Motor.C.resetTachoCount();
        Motor.B.rotate(angleForTurning, true);
        Motor.C.rotate(-1*angleForTurning);
        Motor.B.suspendRegulation();
        Motor.C.suspendRegulation();
        return;        
    }
 
}
Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License