LEGO Mindstorms and Rev Robotics

LEGO Mindstorms and Rev Robotics
The first library to control Mindstorms Robots from Rev is born at RunRevLive

A few years ago now, LEGO updated their Mindstorms robotics range and released Mindstorms NXT. This added a quite a lot of new features such as a much more powerful microprocessor in the control brick, stronger, more controllable motors and the ability to communicate with the control brick via BlueTooth either from a computer or from a mobile phone with the right Java app installed.

Even more important than all of this, for programmers and hackers, was public documentation of the communication protocols along with both software and hardware developer kits. Many people had hacked around with the older kits, but this time LEGO explicitly gave out the information (and permission) to do so. By now there are half a dozen different firmwares/OSs that can be run on the control brick, including variations on Java and C environments.

A few years ago I was looking for a control and motor system that could be used to power a motorised tripod head for shooting 360 degree panoramas from the top of a tall pole. Mindstorms NXT plus some off-the-shelf aluminium structural members did the trick quite nicely. I ended up with a motorised head which you could control via a mobile phone, but not in a very sophisticated manner - more than ‘drive’ and ‘start running program x’, but not by much.

So phone-to-brick communication works, but the plan had always been to use Rev to build a more fully-featured interface for direct computer control, via serial port communications, as you can set up virtual serial ports for BlueTooth devices on OS X. Add in a variation in the form of a wheeled LEGO robot for shooting low-level panos and computer control gets even more attractive.

Unfortunately my knowledge and experience of working with serial ports (and the type of hex codes that the protocol requires) is a bit minimal. I had a few attempts at communicating with the virtual serial port but never with any success, even when trying Sarah Reichelt’s excellent SerialTest stack. Cue RunRevLive 2009! If there was any place on the planet that would have the right mix of people to sort out Rev serial port communications, this would be it!

So, armed with the NXT brick, a MacBookPro and a whole bunch of PDF documentation for the NXT Bluetooth protocol the search began in earnest, gathering up interested spectators such as Björnke von Gierke and Charlie Faddis along the way...
After trying numerous variations of settings in Sarah’s test stack, based on web searches for the baud rate etc, nothing. Then I realised that Sarah’s stack was trying to open the port /dev/cu.Panobot-DevB-1 when it should (according to the net) have been /dev/tty.Panobot-DevB-1. Hurrah! Errm, well, actually... no difference. :-(

Interlude - at this point, I feel I should mention that although the NXT communications protocol is comprehensively documented, it’s hard to make head-or-tail of if your main experience of programming is a high-level language like revTalk. The docs are geared towards people who are dealing with low-level hardware devices, and involve large chunks of hex strings such as ‘0x00 0x04 0x00 0x50 0x01 0x01 0x00 0x20 0x00 0x00 0x00 0x01’, with each byte contributing a different part of the command string. Added to this, the BlueTooth part of the protocol meant adding extra bytes at the start of the message to tell the brick how long the message was going to be. Eurgh. Plus no mention that the hex codes had to be translated into binary data, for people unused to dealing with hex...

At this point Mark Waddingham (Runrev’s resident genius) got involved in the task (read: ‘Mark, you know about serial ports, don’t you? Not recently? Well, help me anyway, what I’m trying to do is...’). He delved into Sarah’s stack to remind himself about serial ports and had some of the same problems with unknown settings as I’d had. More net searches, reading of Terminal man pages and the NXT PDFs occurred, including what later turned out to be a pivotal discovery - a complete ‘known good’ command string (0x06 0x00 0x80 0x03 0xB8 0x01 0x28 0x00) to make the brick beep.

Eventually, Mark started writing data directly to the driver, opening it as if it were a file. First success - opening the driver is making the two-way communication icon show up on the NXT brick! Then our known-good command string came in, as it already had the length bytes added for the Bluetooth protocol. Using numtochar to turn the hex into binary data, saving it as a file and writing the file to the driver did nothing, though. Hmm. Eventually, it turns out that a handshake command had to be sent to the brick after opening the driver, to tell it to listen out. Still no beep, but this turned out to be user error, as I had long ago turned down the volume on the brick so that it wouldn’t make annoying beeps whenever you pressed a button.

Volume up, send message, beep! Stick it all in a repeat loop and get a single beep. Add in a wait command, and get a single beep at the end of the repeat loop, no matter how long the wait is. Oh dear, looks like it’s just an error beep. Oh bleep.

At this point the afternoon sessions were starting, Mark had his Externals Extended session to do and I was getting despondent - so close yet so far! By the way, Mark had a great anecdote about having to learn to speak faster so that his mouth could keep up with his brain, but going by his reading speed his mouth still has some way to go. I’m a fast reader, normally managing 2-300wpm with non-technical texts - but when it came to reading through man pages and similar documentation even I couldn’t keep up with the speed he was scrolling...

I can’t remember which session was next, but my brain was still niggling away at the LEGO problem. Reading through the PDF docs again, the first two bytes of the 8-byte command string were the length of the message and then zero (0x06 0x00), followed by 0x00 or 0x80 depending on whether you wanted a response to the command, followed by two bytes to control the frequency of the beep and two bytes to control the duration of the beep. Hmm, what if we change one of the duration bytes and the send the message again?

At this point I have to apologise to whoever was giving the talk (Jan Schenkel?) for the long drawn out beep coming from my desk as I failed to turn off the brick via the front panel buttons and eventually had to pull the batteries out to stop it. Oops. On the other hand, this made it clear that it wasn’t an error beep. Huzzah! Time to leave the room before doing further experiments, though...

This was really the breakthrough moment - a command was being sent to the brick and the parameters of the command could be changed with noticeable effect. The rest, as they say, is history. Or at least a load of trial-and-error testing to find out what all the different parameters for the different commands mean. By Day Plus I’d figured out enough to set specific programs on the brick running and in a rather unreliable way control the motors - just enough to drive the Panobot around the office and set it shooting panoramas.

In the copious free time since the conference I’ve started putting together a proper library stack to ease the integration of LEGO control into other stacks.
This involves several hurdles:

1.) will be a work in progress - so far I’m adding in handlers for commands as needed. So to start with it’s motor control as what I want to do is control the robot’s movement.

2.) & 3.) almost combine into one - setting the status of the motors has seven variables so I started off with a function that takes seven variables. For testing purposes I then made a group with a drop-down menu or a text box as appropriate for each variable which made it far easier to work out which variable does what...

Let’s take a look at the motor command string and it’s options. There are a lot of them as we’re dealing with a load of bytes in the command string, all of which are controlling different things. If you’ve got the Mindstorms Direct Commands PDF open, the command name is SETOUTPUTSTATE.

Byte 0: 0x00 or 0x80 - this tells the brick whether it should just perform the command or give us feedback as well, and is common to all the commands where we tell the brick to do something directly.

Byte 3: Power range (-100 to 100) - what power output should the motor be using? Use negative values to rotate backwards.

Byte 4: Mode bit. This is one of the more complex ones as you can have any combination of ‘Motoron’, ‘Brake’ and ‘Regulated’. I’m still not totally sure of the meanings but for accurate movement ‘Brake’ plus ‘Regulated’

Byte 5: Regulation mode - ‘Idle, ‘Motor Speed’ and ‘Motor Sync’. ‘Motor Sync’ is for when you are controlling two motors and want them to run synchronously. Not sure about the others.

Byte 6: Turn Ratio (-100 to 100) - only has an effect when running two motors syncronised.

Byte 7: Run State - ‘Idle’, ‘Rampup’, Running’ and ‘Rampdown’. If you’re used to animation terms, think ‘ease-in’ and ‘ease-out’. Currently I don’t know if these can be combined (or how), so this byte is fixed in the handler.

Byte 8-12: TachoLimit - how far should the motor rotate, in the form of four hex values? ‘0’ will run forever.

As you can imagine, putting all those hex values together is a pain, so I put together the following function:

function NXTlib_motor tReturn, tPort, tPower, tMode, \
       tRegulationMode, tTurnRatio, tRunState, tDistance

Still a bit of a mouthful but it takes in text and normal numbers so that you don’t need to know the hex values - apart from tDistance as I’ve not figured out how to convert from a decimal number to four hex values. For now it will pass through four hex values, or if the variable is left out then it will send ‘run forever’ to the motor.

So if we look at the ‘tester’ group, we can see controls for the different parameters:

To summarise where things are now:
We can open the virtual serial port and talk to the robot.
A variety of commands can be sent to the robot, and information received back, specifically starting a program running, controlling motors, beeps, stopping everything running.
The library stack is (just about) in a distributable state.
A function for translating error codes into the actual error message is in place (NXTlib_GetError).

Known problems/things still to do:
Feedback received back after a command appears to be one message out of phase - send message 1 and get the return info from the previous message, send message 2 and get the return info from message 1 etc.
Translating strings of hex values to and from decimal (for things like beep frequency and duration) is an area where I need help. Help? They told me I needed help, the men in the white coats, but no! I can create a monster without help!!! Err, sorry, I’ll calm down again soon.

A start has been made on getting a list of files and programs stored on the brick (see the NXTlib_GetAllPrograms function) but is suffering both from the delayed return info and from a limited number of ‘handles’ which are getting used up each time I ask for a file name - and the Close command which takes a handle number as a parameter keeps coming back with an error.

Finally, please download the stack, have a play around and let me know how you get on!