spanners/mindstorms-wall-following-robot

LEGO Mindstorms wall-following robot

AI for a wall--following Lego Mindstorms robot

9797 LEGO® Mindstorms Education Base set with 9841 NXT Intelligent Brick running leJOS 0.9.1beta-3.

PHOTO: LEGO Robot

Front and Side-view of Lego Mindstorms NXJ robot showing ultrasonic sensor and bump sensors]

Finite State Machine diagram

Flow diagram of robot finite state machine

CODE: Core wall-following logic

private static void mainLoop() {
    initialiseMotorsAndSensors();
    while(true) {
        driveForward();
        steerWhenWallSensed();
        backOffWhenBumpersPressed();
    }
}

// (NON-BLOCKING)
private static void steerWhenWallSensed() { 
    int sonarDistance = sonar.getDistance();
    if (sonarDistance < TOO_CLOSE) {
        steer(LARGE_ROTATION);
    } else if (sonarDistance < CLOSE) {
        steer(SMALL_ROTATION);
    } else if (sonarDistance > TOO_FAR) {
        steer(-LARGE_ROTATION);
    } else if (sonarDistance > FAR) {
        steer(-SMALL_ROTATION);
    }
}

// (BLOCKING)
private static void backOffWhenBumpersPressed() {
    if (leftBumpSensor.isPressed() || 
            rightBumpSensor.isPressed()) {
        backOff();
        turnRight();
    }
}