WallFollower.java
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
 
public class WallFollower {
    private final static int  
    noObject = 255,
    // Distance thresholds
    XL3    = 15, 
    XL2 = 20,
    XL1 = 25,
    XR1 = 100,
    XR2 = 75,
 
    // Motor direction
    forward  = 1,
    backward = 2,
    stop     = 3,
    // Power
    minPower = 80,
    fastPower = 100,
    slowPower = 30;
 
    private static MotorPort leftMotor = MotorPort.C;
    private static MotorPort rightMotor= MotorPort.B;
 
    public static void main (String[] aArg) throws Exception {
        UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
 
        int 
        distance = noObject, 
        distanceVariation, 
        oldDistance = noObject;
 
        rightMotor.controlMotor(minPower,forward);
        leftMotor.controlMotor(minPower,forward);
 
        while (! Button.ESCAPE.isPressed())
        {           
 
            distance = us.getDistance();
            distanceVariation = oldDistance - distance;
 
            LCD.drawString("Distance: ", 0, 1);
            LCD.drawInt(distance,4,10,1);
 
            if (distance<=XL1) // Close to the wall
            {
                leftMotor.controlMotor(minPower,forward);
                if (distance<=XL3)
                {
                    rightMotor.controlMotor(minPower,backward); // Very close, turn in place
                }
                else if (distance<=XL2) 
                {
                    rightMotor.controlMotor(0,stop);  // close enough, turn 
                }
                else if (distance<=XL1)
                {
                    rightMotor.controlMotor(slowPower,forward);
                    // motr=MFLT; // a bit too close, shallow turn
                }
 
            }
            else  // far from the wall
            {
                rightMotor.controlMotor(minPower,forward);
 
                if (distance>=XR2) // far from wall,
                {
                    //if (distanceVariation<2) // if small variation turn (avoids turning too short on salient angles)
                        leftMotor.controlMotor(0,stop);
                    //motl=MOFF;
                }
                else if (distance>=XR1)
                {
                    leftMotor.controlMotor(slowPower,forward);
                    //motl=MFLT; // a bit too far, shallow turn
                }
 
            }
 
        }
    }
 
}
Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License