Starting with Surveyor's Stereo Vision System board

Introduction

Being computer vision and robotics hobbyist I was always interested in building robots, which are not just equipped with some sensors, but also have cameras which may give them vision. And long before I started with robotics, my passion was always directed to robots which have two cameras, what makes them similar to most living creatures and what opens the opportunity for stereo vision applications.

If you are a scientists or an engineer working for a company doing stereo vision projects, it is not an issue to get some sort of stereo vision setup, which would allow you to experiment and work on your project. But if you are just a hobbyist it may be not so simple to get an affordable system for your hobby projects. In the past most hobbyists did not have much choice, but to build something on their own using two conventional USB web cameras (like I did in the past too). But nowadays we got more choice and instead of struggling with different issues of home-made stereo vision setup, we can get something dedicated for it. The simplest and cheapest 3D camera to try seems to be Minoru 3D web camera. This is actually something similar to what you can do on your own using two regular cameras, since Minoru 3D camera is just an assembly of 2 small USB cameras and a USB hub. Another available stereo camera is Bumblebee 2, which seems however to be directed mostly to professionals, not hobbyists. But these are just cameras, which has nothing to do with robotics (if you don't add additional parts of course). Luckily there is yet another available stereo system, which is aimed not just for computer vision, but also for robotics applications - Surveyor's Stereo Vision System.

Surveyor's Stereo Vision System

First I need to mention that Stereo Vision System (SVS) robotics board is not something close to Lego's NXT or Innovative First's VEX robotics kits (we can see it from SVS pictures obviously). It is not a kit full of building blocks and its box does not come with nice booklets providing building instructions. This is really a board with all the chips, pins, etc. put outside, so you need to be more careful with it and understand what you are going to build. It is aimed for a bit more experienced users. Having some experience with other robotics boards like Qwerk and Phidgets, I was not frightened by SVS too much. And a little note saying "Visit our website for more information", which was put into the box with the board, also was not too much surprising. Luckily the Surveyor's web site provides all the brief information required to start and the rest can be obtained from their technical support forum, which is really supported very nice - you will not stay waiting days to get a reply.

Note: Actually I would say that comparing robotics boards like Surveyor's SVS, Qwerk, Phidgets, etc. with RCX, NXT or VEX is not really fair. These are two different classes of robotics kits aimed to a bit different audience and to a bit different type of projects. Although the switch from one class of robotics kits to another may be a bit frightening for beginners, these robotics boards give more freedom and flexibility in building something really custom and interesting, which is not limited to blocks given with a kit.

So what is SVS? Basically it is a board with two cameras on it, WiFi module, motors' and servos' controllers and capability for connecting some additional sensors like ultrasonic sensors, compass, etc (the full spec may be obtained on the SVS's home page). It really looks like it has all we need to start building a robot equipped with stereo vision.

Although some robotics boards may allow connecting greater amount of different devices to them (like Qwerk, for example), there are two main things I really like in the SVS board - cameras and WiFi module are already on board. This makes a great benefit to it. You don't need to worry about which camera and external WiFi module to get and about different compatibility issues which may arise (like it was with the above mentioned Qwerk). Just power it on and you will get both communication and video working perfectly.

Another important thing to mention about SVS is that it has not just two cameras, but it is comprised of two SRV-1 Blackfin cameras. This is really important since each SRV-1 Blackfin camera is actually an independent device, which has its own processor, memory, etc. and each of them runs its own copy of firmware. The SVS board just puts them on the same board and makes a link between them making one SRV-1 camera operate as master device and the second as slave. The fact that SVS is based on SRV-1 means also that all the software (and firmware as well) written for SRV-1 is valid for SVS also. SVS just adds second SRV-1 module and a WiFi module to the same board.

SVS's software

The SVS board's package includes a CD with RoboRealm installation. "RoboRealm is a powerful robotic vision software application for use in computer vision, image processing, and robot vision tasks". So this is the first software to try and check if communication and video work fine without any issues. Personally I was a bit unlucky with this application and did not get good enough communication and video from SVS. Hopefully these were all the old bugs, which are fixed already. In addition there is number of other software applications available for the SVS board - most of them are available with sources written in different programming languages.

For those, who would like to write their own software, the SVS board provides three options:

• Write software on the firmware level. The board runs uCLinux and all the firmware's source code is open, so you may change them and make your custom build. If you go this road, then it may be worth of starting from here.
• Write application in C, which is executed using built-in C interpreter. If you go this road then you may start with SVS/SRV-1 C Interpreter documentation plus the next instructions for uploading the code to your board.
• Write applications, which manipulate SVS remotely. For doing this you will need to learn details of SRV-1 communication protocol.

The first two options are aimed for those applications, which require autonomous robots not depending on any other software running somewhere else. But this will require some knowledge and experience working on firmware level. The third option is aimed for robots, which are controlled remotely from PC, PDA, phone, etc. In this case you are welcome to choose any programming environment you like and you don't need changing anything at all on the board's side. However, going this road you may still need to know some basics about firmware updating with newer versions, which are released regularly.

As for me my aim was to control my robot remotely, so I've chosen the third approach in writing software. More of it, while building my robot (which I hope will be ready some day soon) and writing its software, I wanted to get set of reusable classes, which could be incorporated into AForge.NET framework and potentially be used by somebody else.

Communication protocol

So, if we are going to write our own software for remote manipulation of SVS board, then we need start with learning the communication protocol. The SRV-1 Control Protocol is very simple and is actually text based - all the commands are comprised mostly of ASCII characters (in some cases some binary values are also used, but only as parameters). To my mind it would be better if something different could be used as a protocol (something with strictly defined packet structure, which includes command code, status code, data size, data itself, etc), but this is the protocol which was chosen by Surveyor corporation initially because of simplicity to test it and then was kept for compatibility with future modifications.

And Surveyor corporation is really right in regards of testing the protocol - it is really simple to do it (at least one benefit of the protocol). You don't need any of the applications mentioned above, which already implement the protocol. All you need is a simple terminal client, which supports raw connection type (for example, PuTTY):

Note: By default SVS board is configured for an adhoc WiFi network with SSID "SRV1" (channel 6). Once you connect to the network, you can communicate with the board, which by default has IP address set to 169.254.0.10. Remember, that SVS board consists of two SRV-1 Blackfin cameras, which are independent devices. Each of these cameras may be accessed by connecting to different TCP port numbers. By default left camera (master) is accessible on port 10001 and the right camera (slave) is accessible on port 10002. So, just connect to the required port number and send commands defined by the protocol - SVS should reply to them.

The SVS board provides different type of replies for different commands. If reply has fixed length, then it always starts with '#' character and no CR+LF ("\r\n") characters are added to the end of it. If reply starts with "##", then it means that the reply may have variable length and it always contains CR+LF in the end. An important thing to note is that SVS does not reply at all to commands, which are not recognized by it - this should be kept in mind when you write your own software and handled properly.

Now, let's try to switch to some programming language (for example, C#) and get communication with SVS board from it. Usually the code will contain some sort of a loop, which gets new commands from a queue, sends them to the board, reads reply and provides it back to user (or discards it if user is not interested in the reply). The basic code may look something like this:

// number of bytes to read at once
int readSize = 1024;
// reply buffer size
int bufferSize = 1024 * 100;
// bytes actually read as a reply
int bytesRead = 0;

// buffer to read reply into
byte[] replyBuffer = new byte[bufferSize];

// create socket to use for communication
Socket socket = new Socket( AddressFamily.InterNetwork,
    SocketType.Stream, ProtocolType.Tcp );
socket.ReceiveTimeout = 5000;
socket.SendTimeout    = 1000;

// connect to SVS/SRV-1
socket.Connect( new IPEndPoint( IPAddress.Parse( ip ),
    Convert.ToInt16( port ) ) );
    
// ...    
    
while ( /* some condition to stop */ )
{
    // get command/request from a queue, for example
    // ..
    
    // send request
    socket.Send( request );
    
    int bytesToRead = Math.Min( readSize, bufferSize );

    // receive first portion
    bytesRead = socket.Receive( replyBuffer, 0, bytesToRead, SocketFlags.None );
    
    // check if response contains image
    if ( ( bytesRead > 10 ) &&
         ( replyBuffer[0] == (byte) '#' ) &&
         ( replyBuffer[1] == (byte) '#' ) &&
         ( replyBuffer[2] == (byte) 'I' ) &&
         ( replyBuffer[3] == (byte) 'M' ) &&
         ( replyBuffer[4] == (byte) 'J' ) )
    {
        // extract image size
        int imageSize = System.BitConverter.ToInt32( replyBuffer, 6 );

        bytesToRead = imageSize + 10 - bytesRead;    
    
        // read the rest
        while ( bytesToRead != 0 )
        {
            int read = socket.Receive( replyBuffer, bytesRead,
                Math.Min( readSize, bytesToRead ), SocketFlags.None );

            bytesRead   += read;
            bytesToRead -= read;
        }
    }
    else if ( ( bytesRead >= 2 ) &&
              ( replyBuffer[0] == (byte) '#' ) &&
              ( replyBuffer[1] == (byte) '#' ) )
    {
        // make sure reply contains CR+LF
        // if not, continue reading from socket like with image reply
        // ...
    }

    // by this time the command reply should be in the buffer
    // and size of reply should be equal to bytesRead
    
    // notify user about reply from SVS
}

Of course the code above is not complete and does not handle different type of exceptions as well as it does not cover the mechanism of communication with user (getting commands and providing replies), but it gives the basic idea of writing communication loop with SVS board.

As I already mentioned before, my aim was to write some reusable classes to communicate with SRV-1 and SVS robots and to incorporate them into AForge.NET framework. The work was done and the classes together with documentation and sample applications are available in 2.1.0 version of the framework (there we can find complete communication code with SRV-1). Using classes from AForge.Robotics.Surveyor namespace it is not required to handle all the communication issues on your own, but just use provided methods to perform most of the robot's control. For many of the commands defined in SRV-1 Control Protocol, the classes provide dedicated methods, which do all the work for you. But in the case if some commands are not represented by corresponding methods, the classes provide methods to directly send a command to SRV-1 device and get a reply. Below is small sample of using SVS class for communication with SVS board:

SVS svs = new SVS( );
// connect to SVS board
svs.Connect( "169.254.0.10" );
// get version string
string version = svs.GetVersion( );
// set video resolution and quality
svs.SetQuality( 7 );
svs.SetResolution( SRV1.VideoResolution.Small );
// ...

The rest of the article is mostly dedicated to two things: 1) how to use AForge.NET framework to access different parts of SVS board (cameras, motors, servos, etc); 2) sharing some experience working with SVS board, which may include some found underwater stones.

Accessing video stream

There are multiple ways of getting access to video frames produced by SVS board. The simplest one is just to take single snap-shot image from one of the cameras using SVS.GetImage() method:

// get image from left camera
Bitmap leftImage = svs.GetImage( SVS.Camera.Left );

The method works fine, but it returns just a single image. This means that if you need to continuously receive video frames from SVS board, then you may need to organize your own loop preferably in a background worker thread, which does constant polling of SVS asking for new frames. To simplify things there is SRV1Camera class, which does all the work for you and provides new video frames through event handler:

// get left camera
SRV1Camera camera = svs.GetCamera( SVS.Camera.Left );

// set NewFrame event handler
camera.NewFrame += new NewFrameEventHandler( video_NewFrame );
// start the video source
camera.Start( );
// ...

private void video_NewFrame( object sender, NewFrameEventArgs eventArgs )
{
    // get new frame
    Bitmap bitmap = eventArgs.Frame;
    // process the frame
}

The SRV1Camera class creates its own background thread and does all the dirty work. Using its FrameInterval property it is even possible to control video frame rate. Of course it will not allow you to receive more frames per second than camera can provide, but it may be used to set lower frame rates leaving camera a chance to do something else (like handle other commands). The class becomes even more useful because of its integration with the rest of AForge.NET framework's classes. Using VideoSourcePlayer control it is possibly not only to receive video frames continuously, but also display video in your application. Just put two VideoSourcePlayer controls on your form and assign left and right cameras to it:

// start left camera
SRV1Camera leftCamera = svs.GetCamera( SVS.Camera.Left );
leftCameraPlayer.VideoSource = leftCamera;
leftCameraPlayer.Start( );

// start right camera
SRV1Camera rightCamera = svs.GetCamera( SVS.Camera.Right );
rightCameraPlayer.VideoSource = rightCamera;
rightCameraPlayer.Start( );

The SVS class provides some additional video related methods, like SetResolution(), SetQuality() and FlipVideo(), which names are quite self describing. From the SRV-1 Control Protocol definition we can see that SRV-1 Blackfin cameras provide some more video related functions, but so far they are not exposed through dedicated methods. To my mind all the extra stuff may be done faster on the PC which manipulates the SVS board, so loading the board with more stuff to do will make it only less responsive to other commands. But if any of the extra functions is still required, then it may enabled by sending direct command to the SVS board:

// get direct access to the left SRV-1 Blackfin camera
SRV1 srv = svs.GetDirectAccessToSRV1( SVS.Camera.Left );
// send direct command "g1" - enable color segmentation 
srv.Send( new byte[] { (byte) 'g', (byte) '1' } );

Manipulating motors

The interface set by SRV-1 Control Protocol defines two ways of controlling motors connected to SVS or SRV-1 robot - using set of predefined commands or using direct control of motors' power. The first method allows to send such predefined commands like "Drive forward", "Drive Left", "Drive Backward", "Increase Speed", "Stop", etc. These commands may be sent using ControlMotors() method of the AForge.NET framework:

svs.ControlMotors( SRV1.MotorCommand.DriveForward );
svs.ControlMotors( SRV1.MotorCommand.IncreaseSpeed );
svs.ControlMotors( SRV1.MotorCommand.DriveLeft );
svs.ControlMotors( SRV1.MotorCommand.Stop );

The second approach of controlling motors is direct control of both motors' speed. The approach is represented by RunMotors() method, which allows to set speed of each motor in the [0, 127] range (if it is required to run motors in opposite direction, then speed value should be negative) and also amount of time to run motors. To simplify things the framework also provides StopMotors() method, which is equivalent to calling RunMotors() with motors' speed set to 0. Below is the sample of usage these methods.

// drive forward for 1 second (100 * 10 = 1 sec)
svs.RunMotors( 70, 70, 100 );
// drive backward forever
svs.RunMotors( -70, -70, 0 );
// turn right (left wheel's speed is greater)
svs.RunMotors( 80, 60, 0 );
// finally stop
svs.StopMotors( );

Which of the two methods to use to control SRV-1/SVS robot's motors? As for me I personally always prefer direct access to motors, since it gives more flexibility and control over them. Experimenting with both approaches on my robot I've found few issues with predefined commands, which make them a bit not so nice in use.

• None of the predefined motors' commands will work unless at least one direct command is used. This means that at least StopMotors() method should be called once before calling ControlMotors() method.
• Predefined commands like "Increase Speed" and "Decrease Speed" don't have any effect until the next driving command. My expectation was that if I use "Drive Forward" command and then "Increase Speed", then the robot should continue driving with higher speed. But it will continue driving with the same speed as before. To get higher speed it is required to send "Drive Forward" command again (or any other driving command).
• It seems that speed settings, which are used for predefined motors' commands, are mostly suited for robots like the original SRV-1 robot, which is not very big. For bigger robots, like I had, the "Drive Forward" command may not have any effect at all unless few "Increase Speed" commands are sent. This fact brings all sorts of other issues. For example, the commands "Rotate left/right by 20 degrees" don't work accurately at all on bigger robots even with high speed settings, since motors' power and time for rotation were not calculated for such robots.

It is really hard to create generic predefined commands, which suite every robot needs, and the above issues show it clearly. So I always prefer direct control of motors if robot's API/protocol allows it.

There is one more motors' related method available in the framework (as well as command in SRV-1 Control Protocol). The EnableFailsafeMode() method allows to set which power values to apply to both motors in the case if robot does not receive any commands during last 2 seconds. The method is very useful for instructing robot to stop if communication was lost, for example.

Manipulating servos

Before we start with servos' API, it would be nice to clarify what is the amount of servos could be handled by SVS board and how to connect them properly. These all can be found by learning carefully SRV-1 Control Protocol, markers on the board and Surveyor support forum, but I would better share my findings. First thing to mention is that motors' and servos' control is handled with help of 4 timers (TMR2, TMR3, TMR6 and TMR7). Each SRV-1 module on the SVS board has its own 4 timers, so in total SVS may handle 8 motors/servos.

Now the protocol part. The SRV-1 Control Protocol defines two commands to manipulate servos. Each of the commands may handle two servos - left and right (the "left" and "right" is just a naming convention used in these commands and it has no relation to how servos are used). The first command is "Sab" - controls servos connected to TMR2 (left servo) and TMR3 (right servo). The second command is "sab" - controls servos connected to TMR6 (left servo) and TMR7 (right servo). Since SVS has two SRV-1 modules, sending these commands to each of them may in theory give control to 8 servos.

The SVS setup guide shows pin out for a single servo, so we can find signal, +5V and ground pins. But with the picture below I would like just to clarify which pins are responsible for which servo.

• 1 - Master SRV-1, TMR2 (left servo for "Sab" command);
• 2 - Master SRV-1, TMR3 (right servo for "Sab" command);
• 3 - Master SRV-1, TMR6 (right servo for "sab" command);
• 4 - Master SRV-1, TMR7 (left servo for "sab" command);
• 5 - Slave SRV-1, TMR2 (left servo for "Sab" command);
• 6 - Slave SRV-1, TMR3 (right servo for "Sab" command);
• 7 - Slave SRV-1, TMR6 (right servo for "sab" command);
• 8 - Slave SRV-1, TMR7 (left servo for "sab" command).

Since each command controls two servos, it is convenient to divide all servos' pins into 4 banks:

• Bank 0 - servos 1 and 2;
• Bank 1 - servos 3 and 4;
• Bank 2 - servos 5 and 7;
• Bank 3 - servos 7 and 8.

From all the above it may look like we can connect 8 servos to SVS board and control them all remotely using the commands defined in SRV-1 Control Protocol. But it is not actually true. As it was already mentioned the timers are used not only for servos, but for motors as well. The TMR2 and TMR3 of the master SRV-1 module are reserved to control two motors, which may be connected to the board. If we try connecting servos to the bank 0 and send "Sab" command to the master SRV-1 module, it will not have any effect on servos (but may have effect on motors, since they use these timers). This means that in total SVS may handle only 6 servos, since bank 0 of servos can not be controlled by protocol's commands.

The AForge.NET Framework allows to control 1^st, 2^nd and 3^rd banks of servos using ControlServos() method. All you need to do is to specify which bank of servos you would like to control and the method will resolve on its own which command to send to which SRV-1 module.

svs.ControlServos( SVS.ServosBank.Bank1, 30, 40  );
svs.ControlServos( SVS.ServosBank.Bank3,  0, 100 );

One issues which I've observed with my servos connected to SVS is that I was unable to get rotation range more than 45 degrees. The commands to control servos accept value in the [0, 100] range, which corresponds to [1, 2] ms pulse range. Rotating servos to the position corresponding value 0 (1 ms pulse) and then to the position corresponding value 100 (2 ms pulse) gave me only 45 degrees rotation for my servos. The only way to change the pulse range (which would give wider rotation range) is to change some values is firmware.

Accessing ultrasonic modules

SRV-1 firmware supports Maxbotics EZ0 and EZ1 ultrasonic range finders and each SRV-1 module on SVS board allows connecting up to 4 of them. Although all the details and pictures regarding connecting ultrasonic modules to SRV-1 robot are provided on the Surveyor support forum, I would like to share some of my experience. The soldering instructions given on the forum look fine and will really work, but they seem to be like a quick demo. In my opinion chaining ultrasonic modules directly one to another is not a good idea in terms of possible future extensions and changes. If it happens that you need to change/add/remove one of the modules, then you will end up soldering again and again. Instead of direct chaining I soldered each ultrasonic module independently using distinct wire colors for each pin and then connected all the modules using a block. This type of connection allows changing ultrasonic modules setup easily in the future without requiring any further soldering.

The left picture shows wiring for individual ultrasonic module:

• Ground: black-brown wire;
• +5V: black-yellow wire;
• RX: blue-orange wire;
• PW: blue-yellow wire.

The right picture shows the chaining. On the top of it are wires coming from individual ultrasonic modules and on the bottom are wires going to SRV-1 module. Here are their meaning and connection pins on SRV-1:

• Blue wire - 1^st sonar, pin 27;
• White wire - 2^nd sonar, pin 28;
• Orange wire - 3^rd sonar, pin 29;
• Black wire - 4^th sonar, pin 30;
• Green wire - trigger, pin 18;
• Yellow wire - +5V, pin 1 (1^st pin on SRV-1 actually gives 3.3V, but ultrasonic modules seem to work fine with it);
• White wire (right most) - ground, pin 2.

So, as we can see from all the above, each PW signal from ultrasonic module goes independently to a dedicated pin on SRV-1 module, but RX, +5V and ground are chained, so all modules are connected to the same pin on SRV-1. Finally here is how it looks on the SVS board's side:

Now the software part. The SRV-1 Control Protocol defines a command, which allows reading values from all 4 ultrasonic modules simultaneously. The command is wrapped by UltrasonicPing() method from the AForge.NET Framework, which returns values of ultrasonic module measured in inches:

// read values from ultrasonic modules connected to master SRV-1
float[] ranges = svs.UltrasonicPing( SVS.Camera.Left );
// check one of the values
if ( ranges[0] < 15 )
{
    // getting close to an obstacle
    // ...
}

As it was already mentioned, SVS board is comprised of two SRV-1 modules. This means that potentially it is possible to connect up to 8 ultrasonic modules and UltrasonicPing() method will allow reading them all - just need to make one call to read values from master SRV-1 module and then another call to read values from slave SRV-1.

Working with I2C device - compass

Since SRV-1 Blackfin camera supports I2C devices, the SVS board also provides this support and allows connecting two I2C devices - one device for each camera. There are plenty of different I2C devices available, but we'll cover only one of them in this article - compass. For example, let's take a look at Honeywell's HMC6352 compass (can be ordered from SparkFun Electronics or other dealers).

The HMC6352 compass is also a choice of Surveyor corporation, so it is supported by SRV-1/SVS firmware (support by firmware is not actually required, but in the case of this compass the SRV-1 protocol just provides additional command for convenience). Quick instructions on how to connect it and start working with are given on Surveyor's support forum. Here is the list of compass's pins and corresponding pins on SRV-1 camera:

• GND - pin 1 (ground);
• VCC - pin 2 (power);
• SDA - pin 15;
• SLC - pin 14.

Once it is connected, the next step is to get it working. However, the first step to do may sound a bit complicated. Depending on which camera module you have on your SRV-1/SVS board, you may have or you may not have conflict between your camera and compass. The problem is that if you have OV7755 camera module (check label on the module), then it will conflict with compass. But if you have different camera, then it should be fine. The reason of the conflict is that both devices have similar I2C address. Luckily it is possible to change default I2C address of the compass.

Note: Personally I had OV9655 camera module, which does not have any conflicts. This means that in this case I could skip the procedure of changing default I2C address of my compass. But it is still recommended to do this if we want to work with compass using not only I2C commands, but also using special command given by the SRV-1 protocol.

So, let's change default I2C address of compass. If you have OV9655 camera module, then you may leave it as it is. But if you have OV7755 module, then you will need to unplug it from SVS board, so you could work with compass without conflicts during update procedure (if you have other camera modules, then I would suggest checking it with Surveyor corporation or just unplug it anyway for safety). Once camera is unplugged and compass is connected, we need to send the next command to SRV-1 module: 695721770044 (written in hex, but should be sent in binary). Using AForge.NET Framework this can be done this way (suppose that compass is connected to the left camera):

// get left camera of SVS board
SRV1 srv = svs.GetDirectAccessToSRV1( SVS.Camera.Left );
// send 'iW' command - write two bytes to I2C device
srv.Send( new byte[] { (byte) 'i', (byte) 'W', 0x21, 0x77, 0x00, 0x44 } );

Here is what the above code does: write two bytes (0x00 and 0x44) to I2C device ID 0x21 (0x42), register 0x77. The 0x77 register is used for writing data to device's EEPROM memory. 0x00 is an address in memory. 0x44 is a new I2C address of the device.

Note: Very important thing to keep in mind is the way of addressing to I2C devices. There are two common notations of I2C address. The first notation is 8 bit, where the lowest bit of address specifies access mode - reading and writing. Another notation is 7 bit, which does not use lowest bit for specifying access mode. To get 7 bit I2C address from 8 bit address, it is required to shift right value of address by 1 (or divide by 2 for simplicity). For example, let's clarify it on the sample of HMC6352 compass. Default I2C address of this compass is 0x42 in 8 bit notation, which is 0x21 in 7 bit notation. We need to change this address to 0x44, which is 0x22 in 7 bit notation. This all sounds a bit confusing, but we need to keep in mind that SRV-1 communication protocol uses 7 bit notation for addressing I2C devices. However, if the address is specified not in I2C devices' ID field of SRV-1 command, but in data field (like in the above case), then 8 bit notation is used (in the case of compass; for other devices documentation should be checked).

Once I2C address of the compass is changed, we may start reading its values. To do this we need to use 'iR' command (read word - two bytes) provided by SRV-1 communication protocol. We need to read a value from device ID 0x44 (0x22), register 0x41:

// get left camera of SVS board
SRV1 srv = svs.GetDirectAccessToSRV1( SVS.Camera.Left );
// read compass's value
byte[] response = new byte[128];
int read = srv.SendAndReceive( new byte[] { (byte) 'i', (byte) 'R', 0x22, 0x41 }, response );
// convert byte array to string
string str = System.Text.ASCIIEncoding.ASCII.GetString( response, 0, read );

Reply for the above command should look something like "##iR22 2398", where the last number is the value retrieved from compass. As HMC6352 spec says the value is in tenths of degrees from zero to 3599.

As it was already mentioned the HMC6352 compass is supported by SRV-1 firmware, which means we can get value of the compass using special simplified command, which does not require specifying all these I2C addresses and registers (but in this case compass's I2C address should be changed to 0x44 as it was described before). The extra command is '$C':

// get left camera of SVS board
SRV1 srv = svs.GetDirectAccessToSRV1( SVS.Camera.Left );
// read compass's value
byte[] response = new byte[128];
int read = srv.SendAndReceive( new byte[] { (byte) '$', (byte) 'C' }, response );
// convert byte array to string
string str = System.Text.ASCIIEncoding.ASCII.GetString( response, 0, read );

Reply to this command should look something like "##$C 239", where compass value is returned in degrees.

To simplify access to I2C devices, the AForge.NET Framework starting from version 2.1.1 will provide additional methods for reading/writing from/to I2C devices (by the time of writing this update these methods are already available, but 2.1.1 version of the framework is not yet released):

// change compass's I2C address from 0x42 to 0x44
svs.I2CWriteWord( SVS.Camera.Left, 0x21, 0x77, 0x00, 0x44 );
// read compass' value
ushort compassValue = svs.I2CReadWord( SVS.Camera.Left, 0x22, 0x41 );

Everything seems to look nice with the compass, however it is not so in reality. So I would like to mention few things about it. The compass seems to work reasonable well only in the case there are no magnetic disturbances around, which is really hard to avoid building a robot. Most robots are using motors for moving around, which affect compass a lot even if they are not powered. The problem is that if there are any magnets around (like permanent magnets of motors), then compass provides wrong values - it can not properly detect magnetic poles of the planet. Because of this issue it becomes hardly possible to use compass in many real robots. There are few things to try to avoid magnetic disturbances. First thing to try is to place compass as far away from motors as possible. It may sound simple, but it is not, since the distance should be quite big (more than 12 inches for small motors and even more for bigger motors). This approach may work well for robots like YARB (where it was applied), but may not work so well for many others. Another thing to try is to shield motors to get rid of their influence on compass. However, you may end up shielding everything if your robot has many motors. So, I would suggest to think quite carefully about your robot's design before order the compass.

Sample application to work with SVS board

The AForge.NET Framework provides sample applications for controlling both SRV-1 and SVS robots. The SVS sample application (attached to the article) demonstrates how to get video feeds from both cameras, change video resolution and quality, drive robot using predefined commands or direct access to motors, etc.

To do direct control of SVS's motors I reused the same approach, which I used already writing applications for some other robots. I've used two UI controls - the first is aimed to drive robot forward, backward, to the right/left, etc., and the second is used to rotate robot. The first control uses a manipulator which can be dragged using mouse. The further this manipulator is away from centre, the more power is given to motors, which means distance from centre controls speed. And to control direction it is required to deviate this manipulator from centre - if manipulator is dragged straight up from centre, the robot will got forward; if manipulator is dragged a bit to the right, the robot will turn right; if it is dragged down from centre, the robot will do backward movement, etc. The second UI element also uses manipulator - if it is dragged to the right, the robot will rotate right, but if it is dragged to the left, robot will rotate left. And again - distance from centre controls speed.

Conclusion

Personally I really enjoyed working with Surveyor's SVS board and building my robot with it (which I am eager to finish soon). I would strongly recommend trying this board for all those, who have an aim of building a robot equipped with stereo vision and wireless connectivity. Although there are some minor bugs/"features" could be observed, it still gives very positive impression.

Since I was mostly targeted on writing desktop application, which controls SVS robot remotely, my most concern is the text based communication protocol, which does not seem to be very convenient and flexible to me. Since all the commands' codes defined in the protocol are comprised of a single character, the protocol may reach its limit soon and will require an extension. Another issue of the protocol is that it does not seem to expose many of the features of the board. There are lots of things which are not accessible through it and require changing firmware in order to get desired result. This means that all those who do development on firmware level have much more benefits (which is always a case).

Another thing I could wish is to have more analogue/digital inputs/outputs on the board, so it could be possible to connect more sensors and other devices without adding extra controllers. It may require some changes in SVS board's architecture, but could give it yet another bonus and make it even more welcome. But, anyway, it is really nice even in its current shape!