189Fun and Games with Java Lego NXT 2.0

Author: Dr. Heinz M. KabutzDate: 2010-12-12Java Version: 6Category: Tips and Tricks

Abstract: It is almost Christmas time, which gives us an excuse to invest in all sorts of toys. I found that the most ridiculously priced ones are those that promise to have an added benefit besides fun. "Educational", "Good for hand-eye coordination", etc. In this Java newsletter we look at one of these "toys", the Lego Mindstorms NXT 2.0

Welcome to the 189th issue of The Java(tm) Specialists' Newsletter, sent to you from the stormy island of Crete. We harvested our olives last month and now have slightly more than 100 liters of organic extra virgin first cold pressed delicious Cretan olive oil. The farmer who harvested it decided to keep his share for his family instead of selling it, as our oil is so pure. I am a Java programmer, not a farmer, so I neglected the poor trees, hence the "organic" label. You're supposed to use "organic" fertilizer, not no fertilizer. Our yield is small, but oh so delicious.

javaspecialists.teachable.com: Please visit our new self-study course catalog to see how you can upskill your Java knowledge.

Fun and Games with Java Lego

On one of my trips to the USA this year, I purchased the Lego Mindstorms NXT 2.0 robot kit [ISBN B001USHRYI] . My engineer friend Dale was with me at Toys 'R Us when I bought it. He was shocked that I was willing to pay three hundred dollars for a toy. But when he saw the features of this "toy", he realized what a brilliant price this was for a robot, servo motors and a bunch of sensors. If you have kids, or like me are a fan of Lego and Java, then this is your perfect Christmas gift, if you can still get one in time. There, you have my permission to go blow a bundle of cash on a toy and to then pretend it is for research purposes :-) [I hope Helene is not reading this paragraph. Usually she gives up after the first ...]

My son Maximilian and I immediately got to work building the standard models. Also, yesterday I received The Unofficial Lego Mindstorms NXT 2.0 Inventor's Guide [ISBN 1593272154] , which does a fantastic job of explaining how the NXT 2.0 works. I can recommend it. What I like about the book is that it gives tips on Lego construction as well as advanced NXT-G programming tips.

Maxi wanted to remote control a car he had built with the arrow keys on our computer. Since I had no clue how to do this with NXT-G, we installed Java in the form of LeJOS instead. This made programming much easier, especially as LeJOS comes with about 60 sample programs that you can mine for tips on how to do the most diverse of things. Learning a new language or environment is always easier when you can read examples that others have written.

We discovered that it was actually quite easy to read data over a bluetooth connection from the robot. All you do is this:

NXTConnection con = Bluetooth.waitForConnection();
DataInputStream dis = con.openDataInputStream();

My son is not a Java Champion yet, so I build some code that would be extremely easy to understand. The communication protocol simply sends numbers between the PC and the robot via a data stream. We defined the actions like this:

public interface Protocol {
  int STOP = 0;
  int RIGHT = Integer.MAX_VALUE;
  int LEFT = Integer.MIN_VALUE;
  int EXIT = Integer.MAX_VALUE - 1;
  int BLUE = Integer.MAX_VALUE - 2;
  int RED = Integer.MAX_VALUE - 3;
  int GREEN = Integer.MAX_VALUE - 4;
  int NO_LIGHT = Integer.MAX_VALUE - 5;
  // any other positive value is the forward speed and a
  // negative value is the reverse speed
}

Next we wrote some classes that we could use to control the servo motors, the color sensor and the LCD display. We wanted to print to the LCD, so a simple System.out.println() would do just fine:

import lejos.nxt.*;

public class Display {
  public void show(String message) {
    LCD.clear();
    System.out.println(message);
  }
}

Next we wanted a class to control the actual car's servo motors. Our model has two motors, right and left. When we want to go forward, we call the forward() method on both motors and if we want to reverse we call backward(). The speed is adjusted with setSpeed(). Being typical boys, we tried to boost the speed with some gears, but then the torque was too low and we did not really end up going faster.

To turn the car right, we put the right wheel into reverse and the left feel forward. For left we do the opposite. We could optimize this by making the reversing wheel rotate slightly slower. One of the fun things of programming Java on Lego is that you get immediate feedback of what you have produced and you can physically observe when you have made a mistake. This makes it far more real for a 12 year old than showing him jUnit.

Here is our Car model, note that the B servo port is connected to the right wheel and C to the left. We are going to use A to control a gun that Maxi has built, which he got from the Inventor's Guide [ISBN 1593272154] mentioned previously.

import lejos.nxt.*;

public class Car {
  private final Motor rightServo = Motor.B;
  private final Motor leftServo = Motor.C;
  private final Display display;

  public Car(Display display) {
    this.display = display;
  }

  public void forward(int newSpeed) {
    display.show("Forward " + newSpeed);
    setSpeed(newSpeed);
    rightServo.forward();
    leftServo.forward();
  }

  public void reverse(int newSpeed) {
    display.show("Reverse " + newSpeed);
    setSpeed(newSpeed);
    rightServo.backward();
    leftServo.backward();
  }

  public void setSpeed(int speed) {
    rightServo.setSpeed(speed);
    leftServo.setSpeed(speed);
  }

  public void turnRight() {
    display.show("Turn Right");
    rightServo.backward();
    leftServo.forward();
  }

  public void turnLeft() {
    display.show("Turn Left");
    rightServo.forward();
    leftServo.backward();
  }

  public void stop() {
    display.show("Stop");
    rightServo.stop();
    leftServo.stop();
  }
}

We also figured out ways to shine different colours with the color sensor, so we have a Lamp class. The method flashColors() at the end of the class shows each of the lights, followed by a 300 millisecond sleep.

import lejos.nxt.*;

public class Lamp {
  private final ColorLightSensor cs = new ColorLightSensor(
      SensorPort.S3, ColorLightSensor.TYPE_COLORNONE);
  private final Display display;

  public Lamp(Display display) {
    this.display = display;
  }

  public void noLight() {
    display.show("Light Off");
    cs.setType(ColorLightSensor.TYPE_COLORNONE);
  }

  public void blueLight() {
    display.show("Blue Light On");
    cs.setType(ColorLightSensor.TYPE_COLORBLUE);
  }

  public void redLight() {
    display.show("Red Light On");
    cs.setType(ColorLightSensor.TYPE_COLORRED);
  }

  public void greenLight() {
    display.show("Green Light On");
    cs.setType(ColorLightSensor.TYPE_COLORGREEN);
  }

  public void flashColors() {
    try {
      blueLight();
      Thread.sleep(300);
      redLight();
      Thread.sleep(300);
      greenLight();
      Thread.sleep(300);
    } catch (InterruptedException e) {
      // For an explanation why we should do this, please read
      // https://www.javaspecialists.eu/archive/Issue146.html
      Thread.currentThread().interrupt();
    }
  }
}

The class BlueRover receives ints over bluetooth and uses the protocol values to steer the car. Here is the class, again, very easy to understand, even for a 12 year old. As our code gets more complicated, we will refactor it and introduce a state machine. For example, at the moment, the only way to stop the car from turning right is to send a forward or reverse command, which then also changes the speed.

import lejos.nxt.comm.*;
import java.io.*;

public class BlueRover {
  public static void main(String[] args) throws Exception {
    new BlueRover().start();
  }

  private final Display display = new Display();
  private final Car rover = new Car(display);
  private final Lamp lamp = new Lamp(display);

  private void start() {
    try {
      display.show("Waiting...");
      NXTConnection con = Bluetooth.waitForConnection();
      display.show("Connected");

      DataInputStream dis = con.openDataInputStream();

      while (processCommand(dis.readInt())) ;

      rover.stop();
      lamp.flashColors();
      display.show("Disconnected");
    } catch (Throwable t) {
      display.show(t.toString());
    }
  }

  private boolean processCommand(int data) {
    if (data == Protocol.EXIT) {
      return false;
    }

    // I don't like this either, but I wanted to have as little
    // code and as few classes as possible.  Once we add a state
    // machine onto the BlueRover, we will refactor this code.
    if (data == Protocol.STOP) {
      rover.stop();
    } else if (data == Protocol.RIGHT) {
      rover.turnRight();
    } else if (data == Protocol.LEFT) {
      rover.turnLeft();
    } else if (data == Protocol.BLUE) {
      lamp.blueLight();
    } else if (data == Protocol.RED) {
      lamp.redLight();
    } else if (data == Protocol.GREEN) {
      lamp.greenLight();
    } else if (data == Protocol.NO_LIGHT) {
      lamp.noLight();
    } else if (data < 0) {
      rover.reverse(-data);
    } else if (data > 0) {
      rover.forward(data);
    }
    return true;
  }
}

Remote Control for the NXT 2.0

When we wrote our first remote control, it would send each key event to the NXT 2.0 over blue tooth using the AWT event thread. This did not work very well, because if you hold down the forward key, it gets buffered and there is no way for you to then avoid a full speed crash into the wall. I thus added this piece of magic that does the actual sending of the command in a separate thread.

Basically, whenever a command is sent, it is put into the commandQueue. However, if it is a speed command, then we check whether there is another speed command already in the queue. Since this pending speed command can then be considered as old, we remove it from the queue. This greatly enhances the game control with this robot.

import java.io.*;
import java.util.*;
import java.util.concurrent.*;

public class CommandSender extends Thread {
  private final DataOutputStream out;
  private final BlockingQueue<Integer> commandQueue =
      new LinkedBlockingQueue<Integer>();

  public CommandSender(DataOutputStream out) {
    super("CommandSender");
    this.out = out;
  }

  public void run() {
    try {
      int data = 0;
      while (data != Protocol.EXIT) {
        data = commandQueue.take();
        sendData(data);
      }
    } catch (InterruptedException e) {
      return;
    }
  }

  private void sendData(int data) {
    try {
      System.out.println("data = " + data);
      out.writeInt(data);
      out.flush();
    } catch (IOException e) {
      e.printStackTrace();
    }
  }

  public void enqueueCommand(int data) {
    if (isSpeed(data)) {
      removePendingSpeedCommands();
    }
    commandQueue.add(data);
  }

  private void removePendingSpeedCommands() {
    Iterator<Integer> it = commandQueue.iterator();
    while (it.hasNext()) {
      int oldData = it.next();
      if (isSpeed(oldData)) {
        System.out.println("Removing " + oldData);
        it.remove();
      }
    }
  }

  private boolean isSpeed(int data) {
    return data > -1000 && data < 1000;
  }
}

The enqueueCommand() is called from the RoverModel class, which allows us to abstract a little bit and avoid having a dependency from our GUI to Lego. Thus I could also put up a webserver on one of my 16 static IP addresses at home, so that I can control the robot from anywhere in the world. Now THAT would be cool. But we warned. I know someone who wired up his entire home with blue tooth devices. He could turn on the lights by SMS. He could also turn the stove off. More importantly, he could also turn it ON. Big mistake. One day he was checking on the status and accidentally turned the stove on and burned down his house. True story.

Here is the RoverModel, which is called by the GUI whenever the user does an action. The speed on the NXT 2.0 does not go higher than 900, but for practical purposes, even 700 would be fast enough.

import lejos.pc.comm.*;
import java.io.*;

public class RoverModel {
  private int speed = 0;
  private final int SPEED_INCREMENT = 100;
  private final CommandSender sender;

  public RoverModel() throws IOException {
    System.out.println("Trying to connect to bluetooth NXT");
    NXTConnector conn = new NXTConnector();

    // Connect to any NXT over Bluetooth
    boolean connected = conn.connectTo("btspp://");

    if (!connected) {
      System.err.println("Failed to connect to any NXT");
      System.exit(1);
    }

    DataOutputStream out = conn.getDataOut();

    System.out.println("Made connection");

    sender = new CommandSender(out);
    sender.start();
  }

  public void backward() {
    speed -= SPEED_INCREMENT;
    if (speed < -900) {
      speed = -900;
    }
    sender.enqueueCommand(speed);
  }

  public void forward() {
    speed += SPEED_INCREMENT;
    if (speed > 900) {
      speed = 900;
    }
    sender.enqueueCommand(speed);
  }

  public void right() {
    sender.enqueueCommand(Protocol.RIGHT);
  }

  public void left() {
    sender.enqueueCommand(Protocol.LEFT);
  }

  public void blue() {
    sender.enqueueCommand(Protocol.BLUE);
  }

  public void red() {
    sender.enqueueCommand(Protocol.RED);
  }

  public void green() {
    sender.enqueueCommand(Protocol.GREEN);
  }

  public void noLights() {
    sender.enqueueCommand(Protocol.NO_LIGHT);
  }

  public void exit() throws InterruptedException {
    sender.enqueueCommand(Protocol.EXIT);
    System.out.println("Shutting down");
    Thread.sleep(1000);
  }
}

Lastly we have the ugliest GUI on the world, which consists of a single HTML label that tells you what the commands are. I don't need a fancy gui, I just want a way to capture key events and send them to the robot. But we could of course JavaFX this and make it super slick with gradients.

Note that there is no direct link between the GUI and the NXT. Also, note that if the application shuts down, we call exit() on the model. This gives the robot a chance to disengage cleanly.

import javax.swing.*;
import java.awt.event.*;
import java.io.*;

public class BlueRoverGUI extends JFrame {
  public BlueRoverGUI(final RoverModel model) {
    super("BlueRoverGUI");
    JLabel label = new JLabel("<html><p>Please use the arrow " +
        "keys to control the NXT.<p>Press b for blue, " +
        "g for green and r for red.<p>Press n to turn the " +
        "colors off.<p>Close the program to exit NXT " +
        "program.</html>");
    add(label);
    addKeyListener(new KeyAdapter() {
      public void keyPressed(KeyEvent keyEvent) {
        switch (keyEvent.getKeyCode()) {
          case KeyEvent.VK_UP:
            model.forward();
            break;
          case KeyEvent.VK_DOWN:
            model.backward();
            break;
          case KeyEvent.VK_LEFT:
            model.left();
            break;
          case KeyEvent.VK_RIGHT:
            model.right();
            break;
          case KeyEvent.VK_B:
            model.blue();
            break;
          case KeyEvent.VK_G:
            model.green();
            break;
          case KeyEvent.VK_R:
            model.red();
            break;
          case KeyEvent.VK_N:
            model.noLights();
            break;
        }
      }
    });
  }

  public static void main(String[] args) throws IOException {
    final RoverModel model = new RoverModel();
    SwingUtilities.invokeLater(new Runnable() {
      public void run() {
        BlueRoverGUI cont = new BlueRoverGUI(model);
        cont.setSize(500, 300);
        cont.setDefaultCloseOperation(EXIT_ON_CLOSE);
        cont.setVisible(true);

        Runtime.getRuntime().addShutdownHook(new Thread() {
          public void run() {
            try {
              model.exit();
            } catch (InterruptedException e) {
              return;
            }
          }
        });
      }
    });
  }
}

And that's it, really. Maxi has a lot of school work, but especially now that it's sleeting outside, he's spending lots of time constructing cool Lego machines. I know for sure that this is educational. What I love though is that I can get him started in Java programming from a young age. One of the ways that I teach him is by giving him a short one page Java program printout with a few obvious mistakes (i / 2 instead of i * 2) and ask him to try to find them. [Actually that's my production code, but don't tell him that, ok? ;-)]

Android Controller

It should be a piece of cake to write an Android application that can control the robot by tilting the phone. Unfortunately I don't have an Android yet, but this will be on my todo list for when I get one. Oh wait, it's Christmas time, maybe I can buy one and use it for, ahem, "research" ... ;-)

Kind regards from a chilly Crete. Two days ago one of my MVP students in Montreal told me it would snow in Crete on the weekend and I laughed at him. Now he is probably laughing. MVP is a marvelous technology where I can teach courses in Canada and USA from the comfort of my conference room at home.

Heinz

P.S. Amazon links for Lego Mindstorms NXT 2.0 robot kit [ISBN B001USHRYI] and The Unofficial Lego Mindstorms NXT 2.0 Inventor's Guide [ISBN 1593272154] .

Comments

We are always happy to receive comments from our readers. Feel free to send me a comment via email or discuss the newsletter in our JavaSpecialists Slack Channel (Get an invite here)

Related Articles

Browse the Newsletter Archive