A Robot-Control Project

We will look at the process by which a .NET Micro Framework device can be connected to a robot and used to control it. As part of that, we will explore how to get started interfacing with a .NET Micro Framework system and take a look at the way that objects can be used to abstract a physical device.

iRobot Roomba

For the robot part of our project, we will use an iRobot Roomba device. The Roomba is one of a range of domestic robots manufactured by iRobot (http://www.irobot.com/). It functions as an autonomous vacuum cleaner and can navigate through an environment, cleaning as it goes.

Figure 10-1 shows a Roomba in action. It has two motors that can move the robot forward in a straight or curved path. They can also rotate the robot on the spot. The robot also has motors that drive the fan and brushes, and sensors that detect when it bumps into an obstacle or approaches an edge. Sophisticated software, which allows the device to perform as a cleaner, controls all this.

Figure 10-1: iRobot Roomba cleaner.

Although useful as a domestic appliance, the Roomba has an additional attraction for the robot hobbyist. Recent versions provide a serial interface that can allow an external computer to interact with the device. The interface is on a connector fitted to the vacuum.

Figure 10-2 shows the connection on the front of the robot. The interface is based on the popular RS232 standard for serial communications but does not operate at the usual voltage levels. Instead of using 12-volt signal levels, it uses 5 volts. This means that to connect a computer to the device, you need to create a voltage converter. However, this is not difficult; instructions are available on the Internet to create a suitable device (http://hackingroomba.com/projects/build-a-roomba-serial-tether/). Once you have connected the interface to a .NET Micro Framework device, you can control the robot with a C# program.

Figure 10-2: iRobot Roomba mini-DIN connection.

Note 

If your primary aim is robotics research rather than home cleaning, you can now obtain a version of the Roomba platform specifically for creating mobile robots. This dispenses with the cleaning mechanism and contains a payload bay into which you can fit your own devices. It also provides a serial interface at standard RS232 levels, to which you can directly connect a .NET Micro Framework device with a serial port. However, the commands that control the platform itself are the same for both versions of the device. (We, the authors, have to say that we have had more benefit from our machine as a cleaner rather than a robot, but we have only just started creating applications for it.)

Getting Started with the iRobot Roomba

The Roomba robot supports the iRobot Serial Command Interface (SCI). This is a set of commands that you can use to control the functions of the device at a high level or a low level. You can send commands to initiate a floor-cleaning cycle, or you can read individual sensors and give drive commands to the motors. Full details of the interface can be found on the iRobot Web site at http://irobot.com/images/consumer/hacker/Roomba_SCI_Spec_Manual.pdf.

To get started, you need to establish serial communications with the robot itself. This can be a tricky process; until you have established the magic combination of data rates and signal connections, you will be unable to do anything.

Note 

If you intend to make a habit of performing any serial interfacing, we strongly advise you to invest in some hardware to help. At the very least, you should have some kind of signal-measuring device to allow you to tell whether two devices are actually connected and signals are being transferred. We regularly make use of a voltmeter to measure the voltage level on signal pins and an RS232 breakout box that can give a visual indication of serial signal levels.

The approach that you should take when developing hardware is one of starting at the bottom and working up. Rather than create an application and then deploy it, you should start with the simplest possible behavior, prove that it works, and then use it in the next stage of the development. This is a form of test-driven incremental development. At every stage, you should have the fewest possible number of reasons why the device should not work.

Verifying a Serial Connection

The first step in your Roomba control program does not connect to anything at all. Instead, it allows you to be sure that the serial communications port on the .NET Micro Framework device is actually doing something.

 private void SerialHardwareTest() {    // Setting up the configuration of the serial port    SerialPort.Configuration serialConfig =       new SerialPort.Configuration(          SerialPort.Serial.COM1,                        // selected port          SerialPort.BaudRate.Baud57600,                 // baud rate          false);                                        // no flow control    SerialPort robotPort = new SerialPort(serialConfig);    byte[] test = new byte[] { 0xaa, 0x55 };    while (true)    {       robotPort.Write(test, 0, test.Length);    } } 

The method SerialHardwareTest creates a serial port with the required configuration and then repeatedly sends data down it. You can use it to verify that a given port pin is producing an output. A voltmeter connected to the data connection should change its reading when the program is running, and the lights on a breakout box should flicker.

Once the serial port has been shown to work electrically, you can start taking steps to use it in your control program. To this end, you can extract the serial port setup into a method that you can use in the final program:

 SerialPort robotPort; public void SetupSerialPort() {    // Setting up the configuration of the serial port    SerialPort.Configuration serialConfig =       new SerialPort.Configuration(          SerialPort.Serial.COM1,                        // selected port          SerialPort.BaudRate.Baud57600,                 // baud rate          false);                                        // no flow control    robotPort = new SerialPort(serialConfig); } 

We will use the serial port robotPort throughout the project to communicate with the robot.

Sending a Simple Message

Once we have proved that the connection is physically present, we can try to use it to send data. Messages in the SCI format are sent to the robot as a sequence of bytes of data. Each message has a characteristic command code followed by a particular number of data bytes, depending on what the message is trying to do.

To send the messages themselves, we can write a SendMessage method:

 public void SendMessage(string name, params byte [] command) { #if DiagnosticOutput     Debug.Print("Sending : " + name); #endif     robotPort.Write(command, 0, command.Length); } 

This method uses a very powerful feature of C#, parameter arrays. This feature allows you to create a method that will accept a variable number of parameters. As an example, to start the Serial Control Interface, the command is a single byte with the value 128:

 SendMessage("StartSCI", 128); 

However, to set the state of the light-emitting diodes (LEDs) on the robot requires four bytes:

 SendMessage("Leds",139, 25, 128, 255); 

Both of these calls are legal calls of SendMessage. The first call would deliver an array of bytes containing a single entry; the second call would deliver a four-byte array. The SerialPort.Write method accepts an array of characters, making it very easy to pass the parameters that SendMessage received straight into it.

Note also that we pass a message name into the SendMessage method. The robot does not use it, but the program prints it when the message is sent. It provides a very useful diagnostic aid. A defined symbol that can switch on or off blocks of code controls the printing of the diagnostic messages. We create the symbol at the top of the program.

 // remove to disable diagnostic output #define DiagnosticOutput 

Later, you may comment out this symbol and remove the diagnostic messages, but for now they will provide a way of seeing what the system is doing.

We can now send our first commands to the robot.

 SetupSerialPort(); SendMessage("StartSCI", 128); SendMessage("PowerOff", 133); 

This code sets up a serial port and then sends two messages. The first enables the remotecontrol mode of the robot and the second turns the robot off. This may seem a strange thing to do, but it is the smallest command that has an observable effect. Once we have made this command work, we can move on to receiving data from the robot.

Receiving Data

A program that is going to control the robot will need to read data from it about the state of the various sensors it provides. The robot sends data in response to query commands; the number of bytes sent depends on the query. We can create a ReadMessage method that will request a block of data from the robot and deliver it for use in the robot program.

 public byte [] ReadMessage(string name, int length, params byte [] command) {    SendMessage(name, command);    System.Threading.Thread.Sleep(100);         // give the robot time to respond    byte[] result = new byte[length];    int resultLength = robotPort.Read(         result,                                // destination buffer         0,                                     // start position         length,                                // number of bytes         10000);                                // timeout   #if DiagnosticOutput    string reply;    if ( resultLength < length )    {       reply = "**";    }    else {       reply = "";    }    for (int i = 0; i < resultLength; i++)    {       reply = reply + result[i] + " ";    }    Debug.Print(reply); #endif    return result; } 

The method ReadMessage will send a command and read a response. It receives the number of bytes in the response, and it returns a byte array containing the Roomba response. We can use it to read the sensor status information by issuing the appropriate SCI command:

 ReadMessage("ReadSwitches", 10, 142, 1); 

The command sequence 142,1 asks the Roomba to supply 10 bytes of sensor data. At the moment, the program does not use the result of the ReadMessage; instead, we just want to see the diagnostic information produced by the method. This takes the form of the value of each byte, with the line preceded by "**" if the response is smaller than expected:

 Sending : ReadSwitches 3 0 0 0 0 0 0 0 0 0 

The preceding example shows the result of a call to the ReadMessage method. The first two bits of the first byte represent the state of the left and right bump sensors. In the example, both these sensors are on, causing their bits to be set high.

Building an Object to Represent the Roomba

We could use the methods we created earlier to take control of the Roomba, sending it commands and reading the results. However, from an object point of view, we could make the Roomba much easier to use by creating an object that represents the robot itself.

 RoombaRobot robot = new RoombaRobot(); 

Rather than requiring the programs to decode the sensor bits and assemble motor commands, we could instead use a robot instance to do these things for us. In this respect, we can regard the command methods created so far as the low levels of a communications protocol, upon which we will build our robot object behaviors. The constructor of the instance will set up its serial port and initialize the robot:

 public RoombaRobot() {    SetupSerialPort();    SendMessage("StartSCI", 128);    SendMessage("Enable User Control", 130); } 

The Enable User Control message is an additional SCI command. We must issue it to enable programs to send control information to the robot. Programs that wish to use the robot can then create an instance of the robot and send it messages.

 RoombaRobot robot = new RoombaRobot(); robot.SendMessage("Leds",139, 25, 128, 255); 

This abstracts some of the lower-level robot behavior into the RoombaRobot class, but you should really look at making the robot even easier for a program to interface with by using C# properties.

Reading Data from the Robot

Rather than issuing low-level commands to the robot itself, it would be easier if we had a method that we could call to request that the robot update its sensor information.

 robot.UpdateSensors(); 

The UpdateSensors method would request the robot instance to load sensor information from the physical robot and then update sensor properties. There are 18 results returned by this request, and the code could copy each of them into a property that the robot instance makes available:

 public void UpdateSensors() {    byte[] sensorData;    sensorData = ReadMessage("Read Sensors", 10, 142, 1);    if (sensorData.Length != 10)    {       return;    }    byte temp = sensorData[0];    rightBumpValue = (temp & 0x01) != 0;    leftBumpValue = (temp & 0x02) != 0;    wallValue = sensorData[1];    leftDirtDetectorValue = sensorData[8];    rightDirtDetectorValue = sensorData[9]; } 

The preceding is an abridged version of the method; it shows how the method uses each data byte to set property values. Note that the robot provides some of the data as bit fields, which the code must extract using logical operators. The code stores the actual values read from the sensors inside the robot instance and exposes them in the form of read-only properties:

 private bool leftBumpValue; public bool LeftBump {    get    {       return leftBumpValue;    } } 

When a program requires the state of the robot it can send an UpdateSensors request to the robot instance to get up-to-date data. We can then read the replies and use them in the control program.

 RoombaRobot robot = new RoombaRobot(); robot.UpdateSensors(); if (robot.LeftBump) {    Debug.Print("Left Bump triggered"); } 

The previous code creates an instance of a robot controller, updates the sensor information, and then prints a message if the robot has hit something that has caused the left bump sensor to be triggered.

You now know how to create a robot and read information from its sensors. Next, we want to perform some actions that will take control of it.

Sending Commands to the Robot

From an object point of view, reading data from the robot is now very easy. The program just has to read the value of a property. The next thing to do is make sending commands to the robot as easy. The program should initiate sending commands by setting a property on the robot:

 private bool dirtDetectLitValue; public bool DirtDetectLit {    get    {       return dirtDetectLitValue;    }    set    {       if (value == dirtDetectLitValue)       {          return;       }       dirtDetectLitValue = value;       updateLeds();    } } 

The robot has a dirt-detect LED that the program can control remotely. The robot software object exposes a DirtDetectLit property that the program can write and read. When it writes a new value, it tests to see if the LED state has changed. If the state has changed, it calls a method to send a message to the robot, which will update the state of the LEDs.

 private void updateLeds() {    byte[] command = new byte[4];    command[0] = 139;    byte temp = 0;    if (dirtDetectLitValue)    {       temp |= 1;    }    if (maxLitValue)    {       temp |= 2;    }    command[1] = temp;    SendMessage("Leds", command); } 

The preceding is an abridged version of the actual method. The complete one assembles three data bytes from different property values. The method can control the brightness and color of some of the LEDs on the robot, and the properties for these LEDs in the RoombaRobot class are of different types to reflect this. Now we can use the robot very simply.

 while (true) {    robot.UpdateSensors();    robot.DirtDetectLit = robot.LeftBump; } 

This code gives control of the DirtDetectLit LED to the left bump detector. When it is pressed, the LED comes on, and when it is released, the LED goes off. Note that we have to make a call to UpdateSensors each time around the loop to ask the robot for the latest sensor information.

This form of behavior is probably not very sensible; if the user of the object sets the state of three LEDs on the robot, the program will send three messages. What we want is a means by which we can buffer a set of changes and send them as a single transaction. We will address this in the next section, when we make the robot class implement a well-behaved thread-safe object.

Object Instances and Threads

We now have a robot that the program can use very easily. Rather than having to assemble messages and wait for replies, we can simply get and set properties on an instance of the class that represents the robot, and the code in these properties will perform the required actions. However, we must address one final issue: thread safety. For the robot class to be genuinely useful, it should behave correctly with threaded applications. That way we can set off one process to look after the battery temperature which shuts down the robot if the power battery overheats, while another thread deals with collisions with obstacles and so on.

The problem is that if these threads all start to use the robot object asynchronously, it could result in some nasty problems. If one thread calls the method to change the state of an LED while another thread is executing the method to read data from the robot, this could result in the corruption of command messages because they are sent out of the single serial port. This would result in invalid data, at best, and the robot would become confused, at worst.

Thread Management with Monitors

One way to resolve this issue would be to make use of Monitor to stop multiple threads from executing a particular method. You first saw this technique in Chapter 4, "Building a Device," when we used it to stop a background and foreground thread from corrupting the flashlight lamp output. By using Monitor, we can make the messages that interact with the robot atomic. For example, in this version of SendMessage, we allow only one thread at a time to send a command to the robot.

 private object syncObject; public void SendMessage(string name, params byte[] command) {    Monitor.Enter(syncObject); #if DiagnosticOutput    Debug.Print("Sending : " + name); #endif    robotPort.Write(command, 0, command.Length);    Monitor.Exit(syncObject); } 

The first thread that calls the Enter method gets a handle to the synchronization object. Successive threads entering the method are held in it until the object is released by the call of Exit and handed on to them. In this way, we can be sure that only one thread at a time runs through the code controlled by the monitor. This means that even if different threads make use of the resources provided by a single robot instance, their requests will not conflict.

However, in this case, there is a better way of addressing this issue, and that is to add further intelligence into the robot instance so that the robot class itself performs all the required serial communication and deals with synchronization itself.

Adding a Robot Management Process

The user of the RoombaRobot class should be completely unaware of the threading issues and sensor-update issues. So at this point, we can start to consider how to make the robot interface as automatic as possible. At the moment, software making use of an instance of RoombaRobot has to make periodic calls to the UpdateSensors method to request that the object update all its internal properties. This is a problem if many external processes want to read values, because unless each process calls the method regularly, it can never be sure if the values are valid. A way to resolve this would be to add a thread that is started when an instance of the class is created. This thread will regularly update the sensor readings so that a client thread can be sure of getting up-to-date information.

 private void robotUpdate() {    while (true)    {       UpdateSensors();    } } 

This method repeatedly updates the sensors as fast as it can. The program starts the thread when it creates a robot instance.

 Thread controlThread; private void setupThread() {    controlThread = new Thread(robotUpdate);    controlThread.Start(); } 

The constructor of the class calls the method setupThread, which starts the thread running. Thus, whenever the program reads values, we know they must be up to date.

However, there is a problem. This works well for getting sensor values, but every now and then, the program must send a command to the robot. Previously, the setting of a property value would directly result in the sending of control information; now, it must request a send action.

 private bool ledsDirty = true; private bool dirtDetectLitValue; public bool DirtDetectLit {    get    {       return dirtDetectLitValue;    }    set    {       if (value == dirtDetectLitValue)       {          return;       }       dirtDetectLitValue = value;       ledsDirty = true;    } } 

The previous code is a modified version of the previous dirt-detect LED driver code. This time, rather than send the new values directly to the robot, a flag, ledsDirty, is set to indicate that the LED's values are out of date and must be sent out to the robot during the next update cycle. We now need to modify the robot update method so that it detects if it needs to update the LED values:

 private void robotUpdate() {    while (true)    {       if (ledsDirty)       {          updateLeds();          ledsDirty=false;       }       UpdateSensors();    } } 

This code tests the ledsDirty flag and calls the update method if it is true. It then resets the flag and continues. This means that we can write very simple code that uses the robot, with no need to perform any communications or update requests.

 while (true) {    robot.DirtDetectLit = robot.LeftBump; } 

This method will make the dirt-detect LED follow the left bump sensor. When this sensor is triggered, the dirt-detect LED will illuminate. This is the simplest possible use of the robot because the user of our object does not have to worry about how the object sends the commands and loads the status; the user can just use the properties to send and receive commands.

Timing Problems with Flags

However, the previous code does have a problem. There is a synchronization issue that will sometimes result in the failure of the program to update the LEDs. Consider the following sequence:

1. The RobotUpdate thread gets control and detects that it needs to update the LEDs because the ledsDirty flag is true. It starts to perform an update.

2. During the update, another thread gets control and changes the state of the LED by changing the value of the LED property. The property method sets the ledsDirty to true, to indicate that the program must perform an update.

3. Then, the RobotUpdate thread gets control back again, completes the update, and sets the ledsDirty flag back to false.

The result of this is that the program will not always update the LEDs correctly, because the update request made during the call of RobotUpdate has been overwritten. The problem occurs because two threads are fighting over a single member variable. We do not know when any thread in the program will be allowed to run, or when it will be paused to let others execute. If the RobotUpdate thread is interrupted while it is updating the ledsDirty value, there is always a chance that it could miss an update request.

Preventing Interruptions Using the Interlocked Class

We could solve the problem using Monitor, but there is another way of addressing this issue that is more appropriate in this situation. The System.Threading namespace provides a class called Interlocked, which exposes methods guaranteed to complete without interruption. The one we need to use is CompareExchange. This method accepts three parameters:

 public static int CompareExchange (           ref int testLocation,           int updateValue,           int targetValue ) 

The first parameter is a reference to the location that the program will test. The second is the update value that the program will place in this location if the test succeeds. The third parameter is the target value that the program uses in the test. The whole method returns the original value of the location. We can use it as follows:

 int ledsDirty; // must use an integer rather than a bool if (Interlocked.CompareExchange(ref ledsDirty, 0, 1) == 1 ) {      // LEDs were dirty but have been reset to zero      // must do update here } 

If the value in ledsDirty matches the target, in this case 1, it is set to the update value, in this case 0. If the original value of the flag was 1, it is time to do an update. The test and reset is a single atomic action, which is not subject to interruption. If the program updates the flag subsequent to the call of CompareExchange, this will be picked up next time the flag is tested. We can make a slight modification to the method to ensure that it detects such updates before it updates the sensors. The modification will slightly improve the speed with which the program changes the LEDs to reflect a property update.

 int ledsDirty = 0; private void robotUpdate() {    while (true)    {       while ( Interlocked.CompareExchange(ref ledsDirty, 0, 1)== 1 )       {          updateLeds();       }       UpdateSensors();    } } 

Robot Events

The next stage will be to allow users of the robot class to bind to events that the robot may generate. We have already considered the use of this technique in Chapter 5, "Developing for the .NET Micro Framework," where the program bound code to events generated by a Global Positioning System (GPS) reader. It would not be difficult to add these behaviors to the robot so that, for example, the program reversed the motor direction in direct response to the robot hitting something. This next phase of the development is left to you.

Performance Tradeoffs

We now have a very easy-to-use robot. A programmer who wants to take the Roomba for a spin only has to think of things for the robot to do, rather than the SCI interface. There are many advantages to an object-based approach like this, which we have already discussed in Chapter 5. For example, we could replace the robot with a completely different one, but if the object that represents it remains the same, we do not have to change code that uses it.

However, our program has grown in complexity to provide this abstraction, and we need to be careful that the completed system still has adequate performance for our target application.

Threads

The program now uses a separate thread to perform the robot interfacing. Most of the time, this thread will be engaged in serial communication, which should mean that it would be paused waiting to receive or transmit bytes, but every thread increases the load on the processor. You need to be sure that the underlying processor can support the required number of threads, and to do this you must perform careful testing. We have run the software on actual hardware, and the performance is presently adequate. We intend to write further diagnostic code to allow us to test the device with further threads.

Latency

As well as the throughput of a system, you also need to consider the latency inherent in it. Latency in this system is the time between an event occurring on the robot and the controlling software delivering a response to it. In the case of the Roomba, this could be the delay between a switch closing to indicate an impact with an obstacle and the software stopping the drive motors.

The robot interface uses a serial communications protocol, which itself provides a degree of latency that is inescapable, but it is important that the design of the software does not impose too much additional delay. In the case of the robot object, we have found the performance to be adequate for most purposes, as long as the speed of the robot is kept at reasonable levels. If you require higher performance, you could achieve this by the use of the robot platform version of the Roomba device, which exposes switch signals directly. You could connect these to interrupt-enabled pins on the .NET Micro Framework processor that could then update the robot state much more quickly. However, it would be the case that the users of the robot class should notice nothing more than an improvement in performance; the abstraction of the behavior should completely hide the fact that the underlying system now works in a totally different way.