Driving a Lego Mindstorms Bot Using C#

• Download demo project - 133 KB
• Download source - 162 KB

Introduction

Robotics topics were always interesting to me, and I always wanted to do something in this area. Nowadays, robots are getting closer and closer to us, migrating from regular industrial robots, which drive production lines, to robots which interact with people and assist them in their daily routines. Of course, robots existing today are getting very complex rapidly, and require a lot of research work to teach them to do some useful work, but if you would like to start in this area, you may want to start with something simple.

Fortunately, there are many different robotics kits available to hobbyists and researchers, which could be used to start quickly in the robotics area. Some of them are simple, some of them are more sophisticated and require more knowledge. We'll start with the simplest kits, I guess, existing today - Lego Mindstorms Robotics Kits. Lego has two robotics kits, which are known as RCX and NXT. The RCX kit was Lego's first robotics kit, and now it does not look to be available in Lego's stores (but still may be found and ordered on eBay or somewhere else), and NXT is the latest kit, which is available in Lego's stores. We'll try both of them ...

Before we start, I would like to note that this article is not related to the topic of robots building using Lego's kits - there are enough manuals and instructions provided by Lego and available on the Internet. The article is going to be dedicated to the topic of manipulating a Lego robot from C# applications using the AForge.NET Framework.

Lego Mindstorms RCX

It looks like Lego has dropped support for its Lego RCX brick already, so it is getting harder and harder to find any documentation about it. A few months ago, it was still possible to find the Lego RCX SDK on the official website (which was quite helpful), but it has gone. So now, all the knowledge may be gathered only from the different websites and projects which still support this device. One of the most helpful resources online about RCX is called RCX Internals, and was put together by Kekoa Proudfoot. There, it is possible to find information about RCX's hardware, about its communication protocol, and other stuff.

To communicate with the PC, the Lego RCX brick uses an infrared communication interface, which requires Lego's IR transceiver to be connected to the PC. The IR tower comes in two variants - early versions were connected through a serial interface to the PC, but the latest versions are connected using an USB interface to the PC. In the case that you have a serial IR tower, you may find the details of the serial protocol on the RCX Internals website, where you may also find a description of all the commands to manipulate the device.

Since serial Lego IR transceivers are quite old, and mostly USB transceivers are in use, the description of the serial communication protocol does not make a lot of use for us. Fortunately, there is an alternative. The RCX SDK includes the GhostAPI, a set of libraries which may be used by application developers to control the Lego RCX brick from their applications. These libraries come as Dynamic Loadable Libraries (DLLs), and may be used from most programming languages. Since we a going to use C#, we are going to use the interoperability services to make use of the GhostAPI.

Well, we'll skip the description of how to interop GhostAPI, since this API is represented by regular DLLs, and there is a lot of information all over the Internet about how to access DLL APIs from .NET languages like C#. Instead of this, we'll describe a bit about how to work with the GhostAPI, taking the already interop-ed API, which is represented by the C# class (methods of this class have the same name as the original GhostAPI functions, so there should be zero confusion regarding the interop-ed version).

The first thing we need to do is to connect to our RCX brick. Connection to the RCX brick is done in three steps:

1. Creating a communication stack where we need to specify the communication port (USB or serial) and the protocol.
2. Selecting the device to communicate to.
3. Connecting to the selected device.

IntPtr stack;
uint status;

// create stack
status = GhostAPI.GhCreateStack(
    "LEGO.Pbk.CommStack.Port.USB",
    "LEGO.Pbk.CommStack.Protocol.IR",
    "LEGO.Pbk.CommStack.Session",
    out stack );

if ( !GhostAPI.PBK_SUCCEEDED( status ) )
    return false;
    
// select first available device
StringBuilder sb = new StringBuilder( 200 );
status = GhostAPI.GhSelectFirstDevice( stack, sb, sb.Length );

if ( !GhostAPI.PBK_SUCCEEDED( status ) )
    return false;

// open communication stack
if ( !GhostAPI.PBK_SUCCEEDED( GhostAPI.GhOpen( stack ) ) )
    return false;
    
// we are connected to RCX ...

Now, when we have a connection to the RCX brick, we may want to send some commands to it, which may instruct the brick to perform an action. The command sending is done in several steps:

1. Creating the command queue
2. Adding commands to the queue
3. Executing the commands' queue
4. Destroying the queue

IntPtr queue;
uint status;

// create command queue
status = GhostAPI.GhCreateCommandQueue( out queue );

if ( !GhostAPI.PBK_SUCCEEDED( status ) )
    return false;

// append command to the queue
status = GhostAPI.GhAppendCommand( queue, command, 
                                   command.Length, expectedReplyLen );

if ( GhostAPI.PBK_SUCCEEDED( status ) )
{
    // execute command
    status = GhostAPI.GhExecute( stack, queue );

    ...
}
    
// destroy command queue
GhostAPI.GhDestroyCommandQueue( queue );

As we can see from the code above, we pass some sort of command variable to GhostAPI.GhAppendCommand(). And, as we may assume, this is something that tells RCX what to do. Our assumptions are correct - the command variable is just a byte array, which contains the command's code and its parameters. And here, the RCX internals website becomes really useful, since it provides a description of all the commands supported by the Lego Mindstorm RCX brick.

For example, if we want our RCX brick to beep twice, we may use the Play Sound request. According to its description, all we need to send is two bytes: the byte of the command code (actually all RCX command codes are encoded with a byte), and a byte parameter which is the sound type to play.

// 0x51 - play sound command comde
// 0x01 - double beep sound type
byte[] command = new byte[] { 0x51, 0x01 };

As we may see from the RCX commands documentation, some commands may return a reply, for example, the state of the RCX sensors. So, the last step after sending a command is to retrieve the reply:

// execute command
status = GhostAPI.GhExecute( stack, queue );

if ( GhostAPI.PBK_SUCCEEDED( status ) )
{
    IntPtr commandHandle;
    uint replyLen;

    // get first command and its reply data length
    if (
        ( GhostAPI.PBK_SUCCEEDED( GhostAPI.GhGetFirstCommand
                ( queue, out commandHandle ) ) ) &&
        ( GhostAPI.PBK_SUCCEEDED( GhostAPI.GhGetCommandReplyLen
                ( commandHandle, out replyLen ) ) )
        )
    {
        // get reply
        status = GhostAPI.GhGetCommandReply( commandHandle, reply, replyLen );
    }
}

Does all the above look complex or confusing? Maybe yes, maybe not. Fortunately, it is not required to study all the details of GhostAPI just in case you want to control your RCX brick from C# - AForge.NET already provides a class to operate your RCX device in a much more friendlier manner. The RCXBrick class allows to perform the most frequently required actions like controlling motors, getting sensors' values, playing sounds, etc.

// create an instance of RCX brick
RCXBrick rcx = new RCXBrick( );
// connect to the device
if ( rcx.Connect( ) )
{
    // set forward direction of motor A
    rcx.SetMotorDirection( RCXBrick.Motor.A, true );
    // set power of motor
    rcx.SetMotorPower( RCXBrick.Motor.A, 1 );
    // turm motor on
    rcx.SetMotorOn( RCXBrick.Motor.A, true );
    // ...
    // turn off motors A, B and C
    rcx.SetMotorOn( RCXBrick.Motor.ABC, false );

    // get first sensor's value
    short value;

    if ( rcx.GetSensorValue( RCXBrick.Sensor.First, out value ) )
    {
        // ...
    }
    // ...
}

The AForge.NET Framework also provides a simple RCX brick test application, which allows to test most of the RCX functionality, and also serves as a simple sample of RCX brick controlling from a .NET application.

Lego Mindstorms NXT

Lego Mindstorms NXT is a new robotics kit from Lego which is actively supported, which makes it easy to find different information about it, robot building recipes, projects which support it, and many other stuff. The device is more sophisticated, supporting a wider range of sensors, which makes it more fun for robot builders, and supporting a more flexible commands set, which provides more opportunities for robot programmers.

Each Lego NXT brick supports communication with a PC over two interfaces: Bluetooth and USB. Since wireless robots are more flexible in their movements and have a chance to be autonomous, we'll concentrate on Bluetooth communication with these devices. The nice thing is that Lego provides information about Bluetooth communication protocols and a description of all commands supported by the device. Although this information is not reachable, it is available on their Lego NXT SDK page.

Note. In case you don't want to study communication protocol details, but would like to use something that hides all the complexity, you may try the Fantom library, which is part of the Lego NXT SDK. The advantage of the library is that it gives you support for all communication interfaces. The disadvantage is that it adds one more dependency to your software, which is not always preferred.

So, we'll go for a manually implemented Bluetooth communication. Fortunately, this is not that complex, like it may sound initially - a PC's Bluetooth adapter may be configured to provide a virtual serial port to communicate with Bluetooth devices. So, all we need is to write some code which communicates over the serial port, sending some commands to the NXT brick in a certain format and receiving replies.

The first thing we need to do is the same as it was before in the case of RCX - connect to our device. Since we are using a virtual serial port to communicate with a device, the first step is extremely simple - just create an instance of the serial port class and open a specific port:

// create serial port, specifying port name (COM8, for example)
SerialPort port = new SerialPort( portName );
// connect to NXT device
port.Open( );

Yes, it is that simple! Now, we need to send a message to NXT asking it to do something. This also may be done as simple as a connection - all we need to do is to send a byte array to the opened serial port.

// message to send to NXT
byte[] message = new byte { ... };

// send 2 bytes of message length
// (suppose we have short message not exceeding 255 bytes length)
byte[] messageLength = new byte[2] { (byte) length, 0 };
port.Write( messageLength, 0, 2 );

// send actual message
port.Write( message, offset, length );

As we may see from the code above, the NXT's Bluetooth communication protocol assumes that, first of all, we need to send two bytes of message length. and then the actual message right after. What is the format of messages to send? The message format is quite simple, and is described in the documents provided by Lego on the NXT SDK page:

• 1 byte - command type:
• 0x00 - Direct command, which requires reply
• 0x01 - System command, which requires reply
• 0x80 - Direct command, which does not require reply
• 0x81 - System command, which does not require reply
• 1 byte - command code (see the Lego documentation)
• Variable length - command data (depends on the command code)

For example, let's do the same as we did before with RCX - prepare a command to play a tone:

short frequency = 300; // tone frequency in Hz
short duration = 1000; // tone duration in milliseconds

byte[] command = new byte[6];

// prepare command
command[0] = (byte) 0x00; // direct command requiring reply
command[1] = (byte) 0x03; // play tone command
command[2] = (byte) ( frequency & 0xFF );
command[3] = (byte) ( frequency >> 8 );
command[4] = (byte) ( duration & 0xFF );
command[5] = (byte) ( duration >> 8 );

The above command may actually use another type - 0x80, which does not require a reply. The only reason for asking a reply from such a command is to make sure NXT gets the command successfully and processes it.

Since different commands may result in a reply form NXT, we may want to read it:

// read 2 bytes of message length
// - assume we expect a small message
int toRead = port.ReadByte( );
// - skip the second bytes
port.ReadByte( );

// read the actual message
byte[] buffer = new byte[toRead];
length = port.Read( buffer, 0, toRead );

The format of the reply data looks very similar to the command data:

• 1 byte - reply type:
• 0x02 - Reply command received from NXT brick
• 1 byte - command code, which equals to the command code sent previously to NXT
• 1 byte - error code if any or 0 on success
• Variable length - reply data (depends on the command code)

That is all about communicating with the Lego Mindstorm NXT over Bluetooth - looks quite simple, and does not require any additional libraries.

As it was in the case of RCX, all the information about NXT above is just for those of us who want to get some ideas about how communication is done with a Lego Mindstorms NXT device. But, if we just want to start working with our Lego without too much complexity, we may take the AForge.NET framework, which provides the NXTBrick class, allowing us to control motors, sensors, and perform other actions:

// create an instance of NXT brick
NXTBrick nxt = new NXTBrick( );
// connect to the device
if ( nxt.Connect( "COM8" ) )
{
    // run motor A
    NXTBrick.MotorState motorState = new NXTBrick.MotorState( );

    motorState.Power      = 70;
    motorState.TurnRatio  = 50;
    motorState.Mode       = NXTBrick.MotorMode.On;
    motorState.Regulation = NXTBrick.MotorRegulationMode.Idle;
    motorState.RunState   = NXTBrick.MotorRunState.Running;
    motorState.TachoLimit = 1000;

    nxt.SetMotorState( NXTBrick.Motor.A, motorState );

    // get input value from the first sensor
    NXTBrick.SensorValues sensorValues;

    if ( nxt.GetSensorValue( NXTBrick.Sensor.First, out sensorValues ) )
    {
        // ...
    }
    // ...
}

And also, the framework provides an NXT brick test application, which may be used to test communication with an NXT brick, and also serves as a sample for controlling the device from C#.

Driving the Lego Car Bot

Now that we know how to communicate with different Lego Mindstorms bricks, it is time to build something and manipulate it from C#. We'll start with classics, and build some simple car bots.

How are we going to drive our bots setting the required speed and direction? We need to remember that these bots don't have turning left-right wheels for directing them. Instead, they just have two independent motors, which are connected to the right and left wheels. So, if you would like to drive straight, you just need to set equal power to both wheels. If you would like to turn, you need to decrease the power of the wheel in the direction you want to drive. Such bots could be naturally manipulated with a joystick or a gamepad. But, what if we don't have them? Or would like to control these bots with just a regular mouse ...

Well, it is not that complex to write our own "software" kind of joystick. Let's create a circular control and put a manipulator into the center of the control. Clicking the manipulator with the mouse's left button and dragging it away from the control's center will lead to the bot's movement. Now, let's divide the control into two parts: upper part is for forward movement, lower part is for backward movement. So, if we drag the manipulator straight upward from center, the bot will move straight forward. But moving the manipulator straight downward from the center will move the bot straight backward. If we want to turn our bot left/right, then we just need to drag the manipulator into the corresponding direction away from the center. So, dragging the manipulator into different directions should result in the bot moving into a different direction. Regarding movement speed, it is quite simple - the further away our manipulator is from the center, the faster our bot is moving. The picture below demonstrates the look of the control (the upper control).

Dragging the manipulator of our "software" joystick results in firing the control's events, which notify about the manipulator's position changes, providing its Cartesian coordinates (X,Y - position relative to the control's center). All we need to do is recalculate these coordinates into the power of both motors (we'll skip this geometry code here, since it may be found in the attached demo application).

The control we got is quite nice, and allows us to move our car bot forward or backward in different directions and with different speeds. And, all this is just with a regular mouse. However, this control still does not allow us one feature which is extremely useful - turning a bot without moving away (turning staying on place). From the bot's perspective, it is quite simple to do - we just need to set the same absolute power to both motors, but for one motor, the power should be positive, and for another motor, the power should be negative. For example, if we speak about the NXT device where motor's power varies from -100 to 100 (0 - no motion), we may set the power equal to 75 for the left motor and -75 for the right motor - this will result in our bot turning in the clockwise direction. So, this is simple for our bot, but not for our control, which does not allow motors to turn in different directions.

To solve the in-place turning problem, let's create an additional control (second control on the picture above). The control looks like a slider control, where the manipulator is placed in the middle of the control. If we drag the manipulator to the right from, the bot turns in the clockwise direction. If we drag the manipulator to the left, the bot turns in the counterclockwise direction. The further away the manipulator is from the center, the faster movements the bot has.

That's all - two controls, and we may control the movements of our car bot in the way we want, just by dragging our mouse!

To See or not to See?

The car bots are nice for a start, but ... What is the robot if it can see nothing? We definitely need to solve the issue, so let's equip our NXT bot with an "eye" - with a camera. Since we don't want to lose the mobility we got from using Bluetooth communication, we definitely do not want to use wired cameras. This means, we need a wireless camera.

The simplest solution is to take a wireless IP camera, but in most cases, they are not that small, and require more power supply. So, we'll take a regular radio spy camera. Of course, this solution requires a receiver and a video capture device, but it gives us quite a small add-on to our bot. Let's put the camera and the NXT pieces together and attach it our car bot ...

The last step is to bring the view from our camera to our application. The capture device I've used supports DirechShow, what makes it very easy to get access to the video. Using the AForge.NET framework, it only takes a few lines of code to enable video in our application. First of all, we'll put the VideoSourcePlayer control on our form, and then we'll use VideoCaptureDevice to get the frames from our camera:

VideoCaptureDevice videoSouce = new VideoCaptureDevice( deviceMonikerString );

videoSourcePlayer.VideoSource = videoSouce;
videoSourcePlayer.Start( );

Here is how the vision comes ...

And now, all these in action ... (sorry for the interference between my radio camera and the Bluetooth communication - try the camera with another frequency).

Conclusion

Well, some people may say that Lego Mindstorms are very simple devices and don't allow to build sophisticated robots. Yes, they are not complex, and don't allow to plug a lot of stuff - maximum three motors and four sensors. But that is the key - the fact that they are not complex allows starting with robotics quite fast, and does not require additional knowledge of electronics. Just a constructor, which allows you to concentrate on the idea of building your robot. And, taking a look at the Lego Mindstorm website or searching on the Internet, you may find that even with this simple kit, it is possible to do quite a lot. And, very soon, we'll continue doing something different with Lego, as with some other devices ...