The LEGO NXT Tribot can be built straight out of the box with a LEGO Mindstorms NXT kit using the instructions supplied with the kit. It has a two-wheel differential drive and a third castor, or jockey wheel, for balance. It should only take about an hour to build the robot.

If you have read the section on simulation, then you will already have seen the Tribot in the simulator. The simulated version has only a touch sensor at the front. However, the NXT kit comes with a range of sensors, and you can build onto the Tribot to create much more complex robots, as shown in Figure 14-2, which shows a Tribot loaded with sensors.

Figure 14.2. Figure 14-2

The NXT "brick" is a large block with an LCD display and buttons that contains an ARM processor and runs its own operating system. (Microsoft uses the term brick to refer to the central intelligence onboard any robot.) It takes six AA batteries, for a total of 9V. Fully loaded with sensors, it will chew through these batteries in a couple of hours. However, do not use rechargeable batteries because they only provide 1.2V (not 1.5V) even when fully charged.

The brick can accept up to four sensors that plug into the bottom and three motors that plug into the top. The black cables are clearly visible in Figure 14-2.

The robot in Figure 14-2 has an ultrasonic sonar sensor in the front that is used to detect obstacles. (It looks like two eyes and is often used as part of a face in LEGO designs.) A touch sensor pokes out from underneath the ultrasonic sensor and is a last resort in case the ultrasonic sensor fails to see an obstacle.

On top of the robot, mounted on a stalk, is a HiTechnic Compass sensor. It should be well away from the motors and brick to avoid magnetic interference. It works indoors but is a little slow to respond, so it is a good idea to turn slowly or stop and wait after a turn before using the compass direction.

Support for the LEGO NXT was included in the original version of MRDS because of the popularity of LEGO. MRDS is not directly supported by LEGO because they sell their own Mindstorms software, which is effectively a competitor to VPL.

dsshost /p:50000 /t:50001 /m:"ProMRDSConfigTribot.manifest.xml"
/m: "ProMRDSConfigDashboard.xml"

LEGO NXT V2 Services

When you download the LEGO NXT V2 services (available from the MRDS Downloads web page) and run the installation, the source code will be copied into the following folder:

samplesPlatformsLEGONXTV2

Source code is provided for all of the supported platforms. The original LEGO NXT services will not be overwritten when you install the V2 services.

There is a solution file in this directory called nxtbrick.sln that you can open in Visual Studio. This contains all of the services in four different projects.

MRDS Service Versioning

MRDS uses strong versioning to ensure that the various services are all compatible with one another. If you make even minor changes to any of the code while you are editing nxtbrick.sln and then recompile, you might have to do a complete rebuild of all the LEGO NXT V2 services.

This has caused some people trouble in the past because the interdependencies between services are quite complex. If you get errors when you are trying to start MRDS services that are related to version mismatches, it might be necessary to recompile all of the affected services twice. The second compilation is required to pick up dependencies that could not be resolved during the first compilation.

Note that there is no need to change the V2 services at this stage!

To use the V2 services, you simply need to set up the appropriate config files and a manifest. The way that V2 works is quite different from V1.5. In particular, the individual sensors have their own config files, which specify where the sensors are connected on the brick. (In V1.5 the sensor configuration was attached to the brick as shown in Figure 14-7.)

Initially, it is not important to get the sensor configuration correct. You can change it later using the Manifest Editor or simply by editing the config files. However, as with V1.5, you have to specify the correct COM port for the Bluetooth serial connection.

Look in the ProMRDSConfig directory. You will find several config files related to the LEGO NXT V2 configuration:

• Lego.Nxt.v2.Brick.Config.xml

• Lego.Nxt.v2.Drive.Config.xml

• Lego.Nxt.v2.TouchSensor.Config.xml (Sensor1)

• Lego.Nxt.v2.UltrasonicSensor.Config.xml (Sensor4)

• HiTechnicCompassSensor.Config.xml (Sensor3)

There are also several manifests in the directory, but the two of interest are TriBotV2.manifest.xml and MyTriBotV2.manifest.xml. The first one is a simple manifest that just loads the basic Tribot with a differential drive. The second manifest includes all of the sensors listed above (assuming they are attached to the sensor ports as specified in the list).

Unlike the V1.5 services, if you run the LEGO NXT V2 services with no serial port specified, then the browser window does not allow you to edit the setting.

If you open Lego.Nxt.v2.Brick.Config.xml in Notepad, you will see that it is only a short file:

<?xml version="1.0" encoding="utf-8"?>
<NxtBrickState xmlns:s="http://www.w3.org/2003/05/soap-envelope"
xmlns:wsa="http://schemas.xmlsoap.org/ws/2004/08/addressing"
xmlns:d="http://schemas.microsoft.com/xw/2004/10/dssp.html"
xmlns="http://schemas.microsoft.com/robotics/2007/07/lego/nxt/brick.html">
  <Configuration>
    <SerialPort>5</SerialPort>
    <BaudRate>115200</BaudRate>
    <ConnectionType>Bluetooth</ConnectionType>
    <ShowInBrowser>true</ShowInBrowser>
  </Configuration>
</NxtBrickState>

The SerialPort is specified as number 5 in the preceding code. You need to change this to the same outgoing port that you used previously to configure the LEGO NXT Brick service in the web browser. That is the only change required. You can then run your LEGO NXT Tribot using the manifests listed above. For example,

dsshost /p:50000 /t:50001 /m:"ProMRDSConfigTribotV2.manifest.xml"

starts the Tribot and launches the Dashboard (because it is in the manifest). The Dashboard is covered later in this chapter. The LEGO NXT should beep, and then you can use the Dashboard to drive it around.

An interesting point about this configuration is that ShowInBrowser is set to true, so a web browser is displayed showing the details of the brick. An example of the main LEGO NXT V2 Brick service is shown in Figure 14-8 for MyTribotV2.manifest.xml, which includes several sensors.

Figure 14.8. Figure 14-8

If you have not used the web browser interface before, you should poke around and become familiar with it. In particular, look in the Service Directory and click some of the services. Figure 14-9 shows thumbnails of the new V2 Touch Sensor, HiTechnic Compass, Ultrasonic Sensor, and Differential Drive. These interfaces update dynamically and are quite impressive.

Figure 14.9. Figure 14-9

Building a Parallax Boe-Bot is relatively easy but it is time consuming. It is not necessary to work through the entire book Robotics with the Boe-Bot that comes with the robot, so it will not take you the 40 hours indicated on the front of the book. The relevant chapters are as follows:

If you are not familiar with breadboarding, you should read Appendix D in the Parallax book as well.

The instructions in the Parallax book will assist you in building the basic robot, and you can start driving it around immediately, but it will be blind because it has no sensors.

In order to use the Boe-Bot with MRDS, you need to download the relevant instructions from the Parallax website and then build the sensor package on the breadboard area. At the time of writing, this document was available from the Boe-Bot Kit for Microsoft Robotics Developer Studio page on the Parallax website, and it was called MRDS-Bluetooth-Boe-Bot-v1.5.pdf.

Note that some of the I/O port assignments for MRDS do not match those used in the Robotics with the Boe-Bot book, but you might find Chapter 5 (about the whiskers) helpful, as well as Chapter 7 (about the infrared "headlights").

Once you have completed your Boe-Bot, it should look like the image in Figure 14-10.

Figure 14.10. Figure 14-10

Notice that the Boe-Bot in Figure 14-10 has the eb500 Bluetooth module installed (standing up in the middle of the Board of Education) and a lot of circuits in the breadboard area. The BASIC Stamp 2 has 16 I/O pins, allocated to the various devices as indicated in the following table:

As well as building the robot, you must download the monitor program into it. You can do this using the BASIC Stamp Editor that is supplied with the kit.

A revised version of the Parallax software is supplied with the code for this book in the Parallax folder under Chapter14. This updated version has some additional features that you might like to use. It can be found in the directory ProMRDSChapter14ParallaxBoe-Bot and the file is BoeBotControlForMRDS.bs2.

When you double-click the .bs2 file in Windows Explorer, it launches the BASIC Stamp Editor. You do not need to make any changes to the code. Just plug your Boe-Bot into a serial port on your PC and download the program by selecting Run from the menu. If you don't have a serial port, then you can buy a USB-to-Serial device from Parallax.

Once you have downloaded the program, the Boe-Bot will reset itself and start running the program. It will beep once and flash the LED on pin 15. From then on, every time you turn on the Boe-Bot, it will beep and flash. This indicates that the monitor program has started running successfully.

Establishing a Bluetooth Connection

Connecting the Boe-Bot to MRDS via Bluetooth is quite easy and is very similar to the procedure described earlier in the chapter for the LEGO NXT:

1. Plug in your Bluetooth USB dongle or turn on Bluetooth on your PC or laptop.

2. Turn on your Boe-Bot, with the eb500 module plugged into it. No setup is required.

3. Open the Bluetooth applet from the Control Panel or the system tray. It should be called Bluetooth Devices or Bluetooth Configuration.

4. In the Devices tab, click the Add button (refer to Figure 14-3). The Add Bluetooth Device Wizard will appear (refer to Figure 14-4).

5. Click the checkbox that says "My device is set up and ready to be found" and then click Next. Your PC will search for Bluetooth devices in range and should find the eb500. The wizard will display a dialog asking for a passkey.

6. Enter the passkey for the eb500, which is 0000 (see Figure 14-5 for an example, but make sure you enter 0000).

   After entering the passkey, there will be a slight delay while the PC determines what services are offered by the eb500 and creates the necessary virtual devices. Eventually the final dialog of the wizard will be displayed, showing the COM ports that have been allocated. (See Figure 14-6 for an example.)

7. Make a note of the outgoing COM port number. You will need this to configure your Boe-Bot. Then click Finish.

If you have installed both a LEGO NXT and a Boe-Bot, then the Devices tab in the Bluetooth applet should look something like the left-hand diagram in Figure 14-11. The eb500 is the new device and the T-NXT is the existing LEGO NXT.

Figure 14.11. Figure 14-11

The right side of Figure 14-11 shows the COM Ports tab. Note that there are two ports for the Boe-Bot: COM8 and COM9. The outgoing port, COM8, is connected to an A7 serial port. So port number 8 is the one that should be used when setting the Boe-Bot configuration. The ports that you see will probably be different from these. It depends on the hardware installed on your PC.

<?xml version="1.0" encoding="utf-8"?>
<BasicStampState xmlns:s="http://www.w3.org/2003/05/soap-envelope"
xmlns:wsa="http://schemas.xmlsoap.org/ws/2004/08/addressing"
xmlns:d="http://schemas.microsoft.com/xw/2004/10/dssp.html"
xmlns="http://schemas.microsoft.com/robotics/2007/06/basicstamp2.html">
  <Configuration>
    <Delay>0</Delay>
    <SerialPort>8</SerialPort>
  </Configuration>
  <AutonomousMode>false</AutonomousMode>
  <Connected>false</Connected>
  <FrameCounter>0</FrameCounter>
  <ConnectAttempts>0</ConnectAttempts>
  <MotorSpeed>
    <LeftSpeed>0</LeftSpeed>
    <RightSpeed>0</RightSpeed>
  </MotorSpeed>
  <Sensors>
    <IRLeft>false</IRLeft>
    <IRRight>false</IRRight>
    <WhiskerLeft>false</WhiskerLeft>
    <WhiskerRight>false</WhiskerRight>
  </Sensors>
</BasicStampState>

Starting the LEGO NXT Tribot service and the Dashboard service at the same time is an example of orchestration. This can be done by creating an appropriate manifest file. Manifests were discussed in earlier chapters, but a brief explanation is given here again because it is good to know how things work "under the hood."

A manifest is an XML file that contains a list of services to run and the relationships between the services (partners). All services should have a manifest. If you look in the MRDS directory under samplesConfig you will find dozens of manifests supplied by Microsoft. You will also see a lot of config files, which are discussed later.

Manifests are read by the Manifest Loader service when you start a DSS node. The basic structure of a manifest is as follows:

Service List
    Service Record
        Contract
        Partner List
            Partner
            . . .
    . . .

For more information, refer to Service Manifests in the MRDS documentation.

<CreateServiceList>
    <ServiceRecordType>
      <dssp:Contract>http://schemas.microsoft.com/robotics/2006/05/legonxt.html
      </dssp:Contract>
      <dssp:PartnerList>
        <dssp:Partner>
          <dssp:Contract>http://schemas.microsoft.com/robotics/2006/05/legonxt.html
          </dssp:Contract>
          <dssp:Service>lego.nxt.brick.config.xml</dssp:Service>
          <dssp:PartnerList />
          <dssp:Name>dssp:StateService</dssp:Name>
        </dssp:Partner>
      </dssp:PartnerList>
      <Name>this:Brick</Name>
    </ServiceRecordType>

<ServiceRecordType>
<dssp:Contract>http://schemas.microsoft.com/robotics/2006/10/legonxtdrive.html
</dssp:Contract>
      <dssp:PartnerList>
        <dssp:Partner>
<dssp:Contract>http://schemas.microsoft.com/robotics/2006/10/legonxtdrive.html
</dssp:Contract>
          <dssp:Service>lego.nxt.tribot.drive.config.xml</dssp:Service>
          <dssp:PartnerList />
          <dssp:Name>dssp:StateService</dssp:Name>
        </dssp:Partner>
        <dssp:Partner>
          <dssp:Contract>http://schemas.microsoft.com/robotics/2006/05/legonxt.html
          </dssp:Contract>
          <dssp:PartnerList />
          <dssp:Name>legonxtdrive:Nxt</dssp:Name>
          <dssp:ServiceName>this:Brick</dssp:ServiceName>
        </dssp:Partner>
      </dssp:PartnerList>
      <Name>this:legonxtdrive</Name>
    </ServiceRecordType>

<ServiceRecordType>
      <dssp:Contract>http://schemas.microsoft.com/2006/06/lego.nxt.bumper.html

</dssp:Contract>
      <dssp:PartnerList>
        <dssp:Partner>
          <dssp:Contract>http://schemas.microsoft.com/2006/06/lego.nxt.bumper.html
          </dssp:Contract>
          <dssp:Service>lego.nxt.tribot.bumper.config.xml</dssp:Service>
          <dssp:PartnerList />
          <dssp:Name>dssp:StateService</dssp:Name>
        </dssp:Partner>
        <dssp:Partner>
          <dssp:Contract>http://schemas.microsoft.com/robotics/2006/05/legonxt.html
          </dssp:Contract>
          <dssp:PartnerList />
          <dssp:Name>bumper:LegoNxt</dssp:Name>
          <dssp:ServiceName>this:Brick</dssp:ServiceName>
        </dssp:Partner>
      </dssp:PartnerList>
      <Name>this:bumper</Name>
    </ServiceRecordType>

  </CreateServiceList>
</Manifest>

Starting the Simple Dashboard

Go to the bottom of the manifest. Just before the closing tag </CreateServiceList> insert the following code to start the Simple Dashboard supplied by Microsoft:

<!-- Dashboard -->
    <ServiceRecordType>
        <dssp:Contract>http://www.promrds.com/contracts/2007/01/

simpledashboard.html</dssp:Contract>
    </ServiceRecordType>
</CreateServiceList>

As you can see, this code adds a new ServiceRecordType to the manifest. The first line is a comment, and obviously it is not required for the code to work. The format for comments is the same as for HTML. This can be useful for temporarily removing part of a manifest, or if you want to store different options in a manifest, such as running your service with the simulator or a real robot.

If you want to use the Dashboard from the website for this book (www.proMRDS.com or www.wrox.com), then the contract is slightly different:

<dssp:Contract>http://www.promrds.com/contracts/2007/10/dashboard.html
</dssp:Contract>

Notice that the name is dashboard.html and that the month and year are different. You must have installed the software from the book's website in order to use this service. It might be necessary to recompile the service as well.

Alternative Approach for Running the Dashboard

This example shows you how to add the Dashboard to a manifest. However, because the Dashboard establishes its partnerships dynamically at runtime, there is no need to put it in the manifest. In fact, it would be wrong to specify partnerships in the manifest.

You can therefore just use a separate Dashboard manifest on the command line in addition to the manifest for the robot that you want to control. Here is an example:

dsshost /p:50000 /t:50001

/m:"ProMRDSConfigTribot.manifest.xml"

/m:"ProMRDSDashboard.manifest.xml"

Note that the preceding command must be typed all on one line.

An example that uses this approach is provided in the MRDS bin folder for the Boe-Bot. It is called BoeBot.cmd. See the discussion below on creating shortcuts to run your robots.

Save the manifest you have been editing and exit from Notepad. You can verify that the manifest is okay by loading it with the Manifest Editor. Select Microsoft DSS Manifest Editor from the Microsoft Robotics Studio (1.5) folder in the Start menu. Open the manifest that you have created. It should look like Figure 14-14.

Figure 14.14. Figure 14-14

The Manifest Editor is the preferred way to create and edit manifests because it understands the format and syntax of manifests, and lists all of the available services. If you enter a string, such as LEGO, into the textbox at the top of the Services panel on the left-hand side, you can search for just the related services.

In the future, you might prefer to use the Manifest Editor. For example, you can try creating a manifest for the LEGO NXT V2 by using only V2 services. It should look very similar to Figure 14-12, but you will need to do some configuration. If you are not comfortable building your own manifests yet, there is a manifest already supplied as ProMRDSConfigMyTribotV2.manifest.xml. This manifest includes all of the sensors for the robot shown in Figure 14-2 and the web browser in Figure 14-9.

If you are using a Boe-Bot, then you can start with the Parallax.BoeBot.manifest.xml and add the Dashboard to it by following the instructions given earlier. Call your new manifest whatever you like. If you are feeling lazy, just use the MyBoeBot.manifest.xml file already provided for you.

Now that you have created an appropriate manifest, you can use it in a dsshost command. However, typing long command lines to start up a DSS node is painful and prone to error, so you will probably want to create a batch command file that does it for you.

The following instructions are for the LEGO NXT. If you have a Boe-Bot, then you can read through the instructions and simply use the Boe-Bot version of the code that is supplied on the book's website, or you can try to figure it out for yourself. There is a BoeBot.cmd batch file in the MRDS bin folder that corresponds to the RunLego.cmd file in the following paragraphs (which is also supplied if you don't like typing). In the example that follows, the batch file is created in the top-level MRDS folder. However, the sample files in the bin folder are designed to run from there and are a little different.

Using Notepad, create a new file called RunLego.cmd and place it into the top-level MRDS folder. It should contain the following commands:

@ECHO ON
REM Run a Lego NXT Tribot
REM Type Ctrl-C in this window when you want to stop the program.
dsshost -port:50000 -tcpport:50001 -manifest:"ProMRDS/Config/MyTribot.manifest.xml"

Now open an MRDS Command Prompt window and run the command procedure by typing RunLego. Figure 14-15 shows what the command window should look like. (This figure shows a complete test run. Note that when you press Ctrl+C to cancel the program, a prompt asks whether you want to terminate the batch job. You should enter y, but don't do this right now!)

Figure 14.15. Figure 14-15

If the NXT is turned off, or MRDS cannot establish a connection, you will see a red error message in the Command Prompt window. Figure 14-16 shows an example of this error message (without the color, of course). In this case, the LEGO NXT Brick service web page is also displayed. This is a roundabout way to do it, but it's one way to change the configuration of your NXT.

Figure 14.16. Figure 14-16

If you made any typing errors when you created the manifest, then red error messages will also appear in the Command Prompt window from the Manifest Loader. You will also see errors appear if you turn off the LEGO NXT before shutting down the service.

Assuming that you did everything correctly, a Dashboard window will appear, as well as a LEGO NXT Brick window. You will have to enter the COM port number again. This happens because you have copied the original manifest into a different folder, but you did not copy the config files. Look in the ProMRDSConfig folder now and you will see a set of config files.

If you continue to run the Tribot from this batch command file, then this will not be a problem. However, if you start your Tribot by running the original manifest, you will have two sets of config files and things can very rapidly get confusing. This is why it is a good idea to place all of your manifests together in a single Config directory.

In the Dashboard, enter localhost for the machine name and click Connect. (The machine name will change to the actual computer name.) Double-click the LEGO drive in the service list to select it, and then click the Drive button. You can now drive the robot around. When you drag the mouse on the "trackball" (just above the Stop button), the Tribot should drive around. Try it out.

At this stage you will probably want to click File ? Save Settings to remember your remote node, the window position, and so on. When you get tired of playing with the Tribot, press Ctrl+C in the Command Prompt window. Note that closing the Dashboard window does not terminate DssHost.

Driving a robot using the "trackball" in the Dashboard is a little tricky. It is much easier to use a control that is designed specifically for the purpose. If you have a joystick or an Xbox 360 GamePad, then you can use that to drive the robot. The only requirement is that the controller must be supported by DirectX. Simply plug in the joystick or gamepad and run a robot manifest that includes the Dashboard.

The example in Figure 14-17 shows a Logitech Attack 3 USB joystick being used to drive a LEGO NXT Tribot using the Simple Dashboard.

Figure 14.17. Figure 14-17

If you look carefully at the dialog, you will notice that there is an "x" for the leftmost button in the row of "o" symbols. This indicates that the trigger button on the joystick is depressed (and none of the other buttons on the joystick are currently pressed). You can use the trigger so that the robot will only move when the trigger is depressed and the joystick is off-center. Some of the other buttons also have functions, but they are not described here—look in the code for the Dashboard.

If you have an Xbox 360 USB GamePad and a joystick, then it will appear in the drop-down list for Direct Input Devices. Simply select it instead of your joystick. You can use the left-hand thumbstick to drive the robot.

Although the dsshost command is now safely embedded in a command procedure, you can't double-click the command procedure in Windows Explorer to run the program. If you watch the screen carefully, you will see a DOS Command Prompt window appear and then quickly disappear again.

The problem is that the environment for running DSS has not been set up. This is easily fixed by a small modification to the command file. Edit RunLego.cmd by adding the two extra lines shown here:

@ECHO ON
REM Run a LEGO NXT Tribot
REM Type Ctrl-C in this window when you want to stop the program.
setlocal
call "%~dp0sdkenv.cmd"
dsshost -port:50000 -tcpport:50001 -manifest:"ProMRDS/Config/MyTribot.manifest.xml"

The purpose of these extra commands is to run sdkenv.cmd, which sets up the MRDS environment. This creates the necessary environment variables. When you open an MRDS Command Prompt window from the Start menu, these variables are already defined.

From Windows Explorer, double-click RunLego.cmd. This should start the program just as if you had run it from an MRDS command prompt.

Taking this one step further, right-click RunLego.cmd in Windows Explorer and click Create Shortcut from the pop-up menu. This will create a shortcut called Shortcut to RunLego.cmd. You can rename this to whatever you like. For example, you might call it just RunLego. If you right-click this shortcut and then click Properties, you can change the icon. A batch file does not contain any icons, but you can select from the default set of icons in Shell32.DLL.

Finally, you can copy or move the shortcut anywhere you like, such as onto your Desktop. This gives you immediate access to the LEGO NXT Tribot right next to all your other important business applications (see Figure 14-18).

Obviously, you can create other command procedures and icons this way for the Boe-Bot or for the LEGO NXT V2 services. Even more complex command procedures can be used to select among a variety of robots.

Figure 14.18. Figure 14-18

This section contains instructions for building a new service from scratch. The completed service, with several additional features, is available in the ProMRDSChapter14Dance folder.

Create a new directory called ProMRDSDance and then create a new service as per the commands below. Once the service has been created, open the solution called Dance.sln. To open this in Visual Studio, you can either type the filename at the command prompt as in the following example, or double-click it in Windows Explorer:

C:Microsoft Robotics Studio (1.5)>cd ProMRDS
C:Microsoft Robotics Studio (1.5)ProMRDS>mkdir Dance
C:Microsoft Robotics Studio (1.5)ProMRDS>cd Dance
C:Microsoft Robotics Studio (1.5)ProMRDSDance>dssnewservice /service:"Dance"

/namespace:"ProMRDS.Robotics.Dance" /year:"2007" /month:"10"
C:Microsoft Robotics Studio (1.5)ProMRDSDance>cd Dance
C:Microsoft Robotics Studio (1.5)ProMRDSDanceDance>Dance.sln

If you are using Visual Studio Express Edition and you have multiple languages installed, make sure that you select C# in the Choose Application dialog, as shown in Figure 14-19.

Figure 14.19. Figure 14-19

Open Dance.cs and add the following using statement at the bottom of the list:

using drive = Microsoft.Robotics.Services.Drive.Proxy;

In order to actually use the drive proxy, you must also add a reference to the appropriate DLL, which in this case is RoboticsCommon.Proxy. You can right-click the Solution Explorer panel and then click Add Reference. Alternatively, click Project Add Reference. It might take a little while to display the list of DLLs, so be patient. Set the Local Copy and Specific Version properties on the Reference to false.

Scroll down in the code to just below the main port declaration and insert a drive partner as shown here:

[ServicePort("/dance", AllowMultipleInstances=false)]
private DanceOperations _mainPort = new DanceOperations();

#region partners
[Partner("Drive", Contract = drive.Contract.Identifier, CreationPolicy =
PartnerCreationPolicy.UseExisting)]
drive.DriveOperations _drivePort = new drive.DriveOperations();
#endregion

It is a good idea to use regions in the code so that you can collapse sections of code to make it easier to find your way around. It also helps to make the code self-documenting.

This code sets up a generic differential drive partner, which means that you can connect to a specific type of robot at runtime by specifying the appropriate contract in the manifest. Notice that the CreationPolicy is set to UseExisting. This causes the Dance service to wait until the partner becomes available. It would not make sense to create a new instance of the drive partner because there is no way to tell which of the many different robot drive services should be used. It is the responsibility of the Manifest Loader to associate a real differential drive with the generic one at runtime.

Add some constants at the top of the class definition as shown:

public class DanceService: DsspServiceBase
    {
        #region constants
        // Constants for motor speeds and timing
        const int repeatCount = 2;         // Number of times to repeat dance
        const float driveSpeed = 0.5f;     // Speed driving forward
        const float rotateSpeed = 0.25f;   // Speed during rotation
        const int driveTime = 1200;        // Time to drive forward (millisec)
        const int rotateTime = 920;        // Time to rotate
        const int settlingTime = 500;      // Time to wait after each move
        #endregion

The purpose of these constants should be obvious from their names and the comments. Basically, the robot will drive around the sides of a square by driving forward for a certain amount of time, turning for a different amount of time, and repeating this three times. An overall repeatCount enables the entire "square dance" to be repeated.

Because some robots are quite primitive, using timers to control motions is the only option that you have. More expensive robots have wheel encoders that can be used to monitor how much the wheels have turned. The LEGO NXT motors do actually have encoders, but they are not used in this example. This is discussed later.

The numbers listed in the preceding code were determined by trial an error. They are not exact, so the robot will not necessarily traverse a perfect square. They also depend on whether you have fresh batteries in the robot because motor speed can decrease considerably as the battery voltage drops.

The settlingTime is a period of time between moves to enable the robot to settle down. Robots cannot instantaneously change speed and/or direction. Without this settling period, driving forward and turning tend to blend into one another, reducing accuracy.

Go to the bottom of the class now and add the following method called Behavior:

#region behavior

// Iterator to execute the Behavior
IEnumerator<ITask> Behavior()
{
    // Wait for the Lego NXT to initialize, otherwise it will
    // miss the initial command
    yield return Arbiter.Receive(
        false,
        TimeoutPort(10000),
        delegate(DateTime timeout) { });

    LogInfo(LogGroups.Console, "Starting now .");

    // Make sure that the drive is enabled first!
    _drivePort.EnableDrive(true);

    for (int times = 0; times < repeatCount; times++)
    {
        // Drive along the four sides of a square
        for (int side = 0; side < 4; side++)
        {
            LogInfo(LogGroups.Console, "Side " + side);

            // Start driving and then wait for a while
            _drivePort.SetDrivePower(driveSpeed, driveSpeed);
            yield return Arbiter.Receive(
                false,
                TimeoutPort(driveTime),
                delegate(DateTime timeout) { });
            // Stop the motors and wait for robot to settle
            _drivePort.SetDrivePower(0, 0);
            yield return Arbiter.Receive(
                false,
                TimeoutPort(settlingTime),
                delegate(DateTime timeout) { });

            // Now turn left and wait for a different amount of time
            _drivePort.SetDrivePower(-rotateSpeed, rotateSpeed);
            yield return Arbiter.Receive(
                false,
                TimeoutPort(rotateTime),
                delegate(DateTime timeout) { });
            // Stop the motors and wait for robot to settle
            _drivePort.SetDrivePower(0, 0);
            yield return Arbiter.Receive(
                false,
                TimeoutPort(settlingTime),
                delegate(DateTime timeout) { });
        }
    }

// And finally make sure that the robot is stopped!
    _drivePort.SetDrivePower(0, 0);

    LogInfo(LogGroups.Console, "Finished");

}

#endregion

The Behavior method is an iterator. It periodically yields control of the running thread using yield return. When an Arbiter.Receive has received a message, the CCR resumes execution of the task at the point where it yielded. This means that the code is executed sequentially, but not necessarily by the same thread.

The first step is to wait for 10 seconds (10,000 milliseconds). This ensures that the partner service has started. It is not a reliable or efficient way to wait for a partner, but it is used here for simplicity.

Notice the calls to LogInfo(LogGroups.Console, "...") in the code, which are there for debugging and to monitor progress. Various logging methods are available for use with DSS, including LogError, LogWarning, LogInfo, and LogVerbose. In this case, the output will be sent to the console (the MRDS Command Prompt window) just as if you had used Console.WriteLine("..."). If you omit the LogGroups.Console parameter, then the output is sent to the log, not the console. Either way, the output appears in the Console Output web page.

Next are two nested loops. The outermost loop simply repeats the "dance" pattern, while the innermost loop executes the four sides of the square that make up the basic behavior. The code consists of a series of calls to SetDrivePower followed by a wait.

The last piece of code you need to add is to execute the Behavior from the Start method:

protected override void Start()
{
    base.Start();
    // Add service specific initialization here.
    SpawnIterator(Behavior);
}

Try building the service to ensure that you have not made any mistakes. There is no point in running it yet because it needs a drive service for a robot as a partner.

You now have a service that can control a robot, but you need to associate it with an actual robot so that it can run.

Open Dance.manifest.xml, which DssNewService created for you. You should see it in the Solution Explorer panel in Visual Studio and you can open it from there. Visual Studio understands XML and will color-code it for you. Add a new Service Record just above the existing Service Record for the Dance service:

<!-- Use a Simulated robot -->
<ServiceRecordType>
    <dssp:Contract>http://schemas.tempuri.org/2006/06/simulationtutorial2.html
    </dssp:Contract>
</ServiceRecordType>

Notice that the additional service is Simulation Tutorial 2 that comes with MRDS. This creates two robots: a Pioneer 3DX and a LEGO NXT Tribot. Save the manifest. (A complete copy of the manifest is supplied as ProMRDSConfigDance.SimTut2.manifest.xml.)

You can now run this new service by simply starting the Dance program in the debugger in Visual Studio.

After the simulator starts, there is a short delay and then the Pioneer robot will start doing the "dance." Figure 14-20 shows the Pioneer partway through its behavior. The LEGO NXT Tribot (in the foreground) will not move.

Figure 14.20. Figure 14-20

When the Pioneer robot has finished, the MRDS window will look like Figure 14-21. Notice the status messages as it progresses through each side of the square; they include a timestamp and the name of the service. Dance, dance, dance!

Figure 14.21. Figure 14-21

You might be wondering why the Pioneer robot did the dance when this chapter has been dealing with LEGO NXT robots. The reason is that Simulation Tutorial 2 creates the Pioneer robot first. Because the Pioneer has a differential drive, the Dance service immediately partners with it. It is possible to enumerate the available services and select the LEGO service, but that's far too complicated for this simple example. Look in the code of the Dashboard for details about how to list the available services.

<?xml version="1.0" ?>
<Manifest
    xmlns="http://schemas.microsoft.com/xw/2004/10/manifest.html"
    xmlns:dssp="http://schemas.microsoft.com/xw/2004/10/dssp.html"
>
  <CreateServiceList>
    <!-- Create a new Lego NXT service -->
    <ServiceRecordType>

        <dssp:Contract>http://schemas.microsoft.com/robotics/2007/07/lego/

nxt/brick.html</dssp:Contract>
        <dssp:PartnerList>
            <dssp:Partner>
                <dssp:Contract>http://schemas.microsoft.com/robotics/2007/07/

lego/nxt/brick.html</dssp:Contract>
                <dssp:Service>legov2.nxt.brick.config.xml</dssp:Service>
                <dssp:PartnerList />
                <dssp:Name>dssp:StateService</dssp:Name>
            </dssp:Partner>
        </dssp:PartnerList>
        <Name>Brick</Name>
    </ServiceRecordType>
    <ServiceRecordType>

        <dssp:Contract>http://schemas.microsoft.com/robotics/2007/07/lego/

nxt/drive.html</dssp:Contract>
        <dssp:PartnerList>
            <dssp:Partner>
                <dssp:Contract>http://schemas.microsoft.com/robotics/2007/07/

lego/nxt/drive.html</dssp:Contract>
                <dssp:Service>legov2.nxt.drive.config.xml</dssp:Service>
                <dssp:PartnerList />
                <dssp:Name>dssp:StateService</dssp:Name>

</dssp:Partner>
            <dssp:Partner>
                <dssp:Contract>http://schemas.microsoft.com/robotics/2007/07/

lego/nxt/brick.html</dssp:Contract>
                <dssp:PartnerList />
                <dssp:Name>Nxt</dssp:Name>
                <dssp:ServiceName>Brick</dssp:ServiceName>
            </dssp:Partner>
        </dssp:PartnerList>
        <Name>legonxtdrive</Name>
    </ServiceRecordType>

    <!-- Use a Simulated robot -->
<!--
    <ServiceRecordType>

        <dssp:Contract>http://schemas.tempuri.org/2006/06/

simulationtutorial2.html</dssp:Contract>
    </ServiceRecordType>
-->
    <ServiceRecordType>
        <dssp:Contract>http://www.promrds.com/contracts/2007/10/

dance.html</dssp:Contract>
        <dssp:PartnerList>
            <dssp:Partner>

                <dssp:Contract>http://www.promrds.com/contracts/2007/10/

dance.html</dssp:Contract>
                <dssp:Service>dance.config.xml</dssp:Service>
                <dssp:PartnerList />
                <dssp:Name>dssp:StateService</dssp:Name>
            </dssp:Partner>
        </dssp:PartnerList>
    </ServiceRecordType>
  </CreateServiceList>
</Manifest>

<?xml version="1.0" encoding="utf-8"?>
<Manifest
    xmlns="http://schemas.microsoft.com/xw/2004/10/manifest.html"
    xmlns:dssp="http://schemas.microsoft.com/xw/2004/10/dssp.html"
    >

  <CreateServiceList>

    <!-- Start BasicStamp2 Brick -->
    <ServiceRecordType>
      <dssp:Contract>http://schemas.microsoft.com/robotics/2007/06/

basicstamp2.html</dssp:Contract>
      <dssp:PartnerList>
        <!-- Initial BasicStamp2 config file -->
        <dssp:Partner>
          <dssp:Service>Parallax.BoeBot.Config.xml</dssp:Service>
          <dssp:Name>dssp:StateService</dssp:Name>
        </dssp:Partner>
      </dssp:PartnerList>

    </ServiceRecordType>

    <!-- Start the BoeBot drive service -->
    <ServiceRecordType>
      <dssp:Contract>http://schemas.microsoft.com/robotics/2007/06/

bsdrive.html</dssp:Contract>
      <dssp:PartnerList>
        <!-- Initial Drive Configuration File -->
        <dssp:Partner>
          <dssp:Service>Parallax.BoeBot.Drive.Config.xml</dssp:Service>
          <dssp:Name>dssp:StateService</dssp:Name>
        </dssp:Partner>
      </dssp:PartnerList>

    </ServiceRecordType>

  </CreateServiceList>

</Manifest>

-port:50000 -tcpport:50001 -manifest:"ProMRDS/Config/Dance.manifest.xml"

copy "$(ProjectDir)dance.manifest.xml" "$(ProjectDir)....Config"

Watch carefully as the robot attempts to inscribe a square on the floor. If you have fresh batteries and you are lucky enough to have a well-matched pair of motors, then the robot might do a reasonably good job. However, most people find that the robot does not drive around a square at all.

Stop the debugger and run the program again. Do this several times. What do you notice? It is quite probable that each time you run the program you get a different result. Even though it only does two iterations of the square, it is unlikely that your robot will end up where it started or even be facing in exactly the same direction. (Of course, you have not tuned the values of timing parameters, but that isn't the point.)

If you look closely, you will notice that when the robot is supposed to be driving forward it often veers slightly to one side—usually the same side—every time it moves forward. This consistent error in the motion of the robot accumulates into larger and larger errors in the pose (position and orientation) of the robot the more times it dances around the square. Welcome to the real world.

There are many reasons for these errors—motors that are not exactly matched in terms of their performance; wheel slippage; slightly different wheel diameters; motors or axles that are not aligned squarely; and so on. You cannot avoid these problems. The best you can do is try to design your application so that the errors are reduced, compensated for, or not relevant.

What you have seen here is an open-loop control system—there is no feedback to help the robot perform its task correctly. If anything goes wrong, the robot has no way to determine that. Contrast this with the PID controller described in the previous chapter.

Many robots rely on reactive behaviors. This means that the robot constantly reacts to what it sees and acts accordingly. For example, following a wall and even wandering around a maze can be done via reactive behavior. In this case, it does not matter much if the robot can drive straight or not because the control program continually adjusts the robot's heading and speed so that it does not bump into walls.

Conversely, if your objective is to build an accurate map of the environment, then you will want the robot to know exactly where it is at all times. Otherwise, the information in the map will be wrong. This task is known as simultaneous localization and mapping, or SLAM. (In Europe it is also known as concurrent mapping and localization, or CML). It is an active area of research.

Localization refers to the process of finding where you are on a map. Unfortunately, if your objective is to build a map, then you don't have a map to begin with! This is a "chicken or the egg" situation, i.e., which comes first? This is why SLAM includes the word "simultaneous."

Without putting too fine a point on it, performing SLAM with the limited capabilities of the LEGO NXT or the Boe-Bot is not really possible. One of the requirements for SLAM that is often overlooked is the information content necessary to produce reliable localization. A laser range finder typically has a 180-degree field of view, with measurements taken every degree or even half a degree and distances measured to an accuracy of about 1cm or 2 cm out to a range of 20 meters. Contrast this with the LEGO NXT Ultrasonic Sonar, which is one single data point (not 180 measurements) with a maximum range of 255cm and an accuracy of only ±3 cm. If you were planning to have your robot map your house, you might want to reconsider.

However, if your robot is operating in a fairly small environment and you already have a map, then you might be able to localize by taking a series of readings. Provided that the robot can turn around accurately, even half a dozen distance readings might be sufficient to localize itself against an existing map.

If you are using a Boe-Bot, however, there is more bad news. The IR sensors on the Boe-Bot are not range sensors—they cannot provide reliable distance measurements. Therefore, it is almost impossible to localize a Boe-Bot or build a reasonable map. Of course, none of this matters if your objective is to learn about robotics and have some fun.

Part of the problem with the original "square dance" is that it is highly unlikely to be square. Using timers to control motions is not very accurate for several reasons:

These problems mean that there is no simple relationship between the running time of the motors and the distance traveled.

This is where wheel encoders might seem like a good solution. A wheel encoder measures a wheel's amount of rotation. This is usually specified as a number of "ticks per revolution." In the case of the LEGO NXT motors, for example, the encoder can be set to either 6 or 360 ticks per revolution.

Unfortunately, the Boe-Bot does not have wheel encoders, so it has no option except to use timers. This leaves you with two options: let the PC do the timing or let the robot do the timing. It is more accurate to get the robot to time the moves, but there are disadvantages as well, as discussed in Chapter 16.

There are two methods defined by the generic Differential Drive that are designed to take advantage of wheel encoders: DriveDistance and RotateDegrees. As its name suggests, DriveDistance can be used to drive the robot forwards or backwards a specified distance (in meters, with negative values meaning backwards). RotateDegrees rotates the robot by turning the wheels in opposite directions. Counterclockwise rotations are positive, and clockwise is negative.

It is easy to modify the Dance service to use these new methods:

If the world worked perfectly, then the preceding code would be all that you need. However, there is a problem with the LEGO NXT V2 services: Sometimes the robot stalls before reaching its target destination. This happens because the motor speed is ramped down as the target is approached in order to avoid overshooting it. However, below a certain power level, there is not enough torque to keep the robot moving and it stalls. This is why the waitForMoves flag was added to the code—to turn off this behavior.

To overcome this problem but still take advantage of the wheel encoders on the LEGO NXT, the following section of code is a hybird of the timed moves and the exact moves. If you look carefully, you will see that it is the same code that was originally used for the timed moves except that now the DriveDistance and RotateDegrees requests replace the SetDrivePower requests if controlledMoves is set to true:

else
        {
            // Start driving and then wait for a while
            if (controlledMoves)
                _drivePort.DriveDistance(driveDistance, driveSpeed);
            else
                _drivePort.SetDrivePower(driveSpeed, driveSpeed);

            yield return Arbiter.Receive(
                false,
                TimeoutPort(driveTime),
                delegate(DateTime timeout) { });

            // Stop the motors and wait for robot to settle
            _drivePort.SetDrivePower(0, 0);
            yield return Arbiter.Receive(
                false,
                TimeoutPort(settlingTime),
                delegate(DateTime timeout) { });

            // Now turn left and wait for a different amount of time
            if (controlledMoves)
                _drivePort.RotateDegrees(rotateAngle, rotateSpeed);
            else
                _drivePort.SetDrivePower(-rotateSpeed, rotateSpeed);

            yield return Arbiter.Receive(
                false,
                TimeoutPort(rotateTime),
                delegate(DateTime timeout) { });

            // Stop the motors and wait for robot to settle
            _drivePort.SetDrivePower(0, 0);
            yield return Arbiter.Receive(
                false,
                TimeoutPort(settlingTime),
                delegate(DateTime timeout) { });
        }
    }

The time delays in this new code are simply to wait for the motions to complete. You need to make sure that they are long enough. For example, change both time delays to 1500. That way, the settling period between driving and turning might be unnecessary. However, stopping the motors after each motion is still a good idea in case the motor was stalled.

Try out this new code and see if it improves the square-drawing ability of your robot. Sometimes it might draw close to a perfect square, but if you run it often enough, you will probably see that it draws some misshapen squares as well, so using wheel encoders is not the perfect solution you might have expected.

Children love lights and sounds. The Boe-Bot has two LEDs (and you could add a third one using the spare I/O pin) as well as a speaker. The LEGO NXT brick has a speaker but no lights. (Although you can display text messages with the LEGO NXT V2 services, they are hard to read on the LCD display.)

Unfortunately, MRDS does not define a generic service that can be used to manipulate digital I/O ports or play tones on a robot. Therefore, you have to use the Brick services directly.

To connect to a Brick, you need to establish a partnership. The process for this is quite simple:

The first step in the Boe-Bot code is to figure out the binary pattern for the two LEDs so that they display the side number for the square (as well as look pretty).

The Boe-Bot uses a microcontroller called a BASIC Stamp 2 to control the robot. Microcontrollers typically have several digital I/O ports that are grouped together in bytes or words. The BASIC Stamp 2 on the Boe-Bot has 16 I/O pins. These can be configured as either inputs or outputs. In theory, you should set pins 14 and 15 to output mode before using them, but this is the default mode.

The SetPins request accepts two pin numbers and the operations to perform on those pins. (It is modeled directly on an internal function that already existed in the Parallax service). The operations that can be performed are defined in an enum called PinOperations, as shown in the following table:

The NO_OP value enables the manipulation of a single pin. The others are self-explanatory.

The last step in the Boe-Bot code is to call PlayTone. This has been deliberately designed to have the same parameters as the PlayTone request for the LEGO NXT, i.e., frequency and duration. Recall that there are no standards yet for bricks (see Chapter 17). Using the same signature, however, enables the code to be the same regardless of which brick is used. However, due to the protocol used by the Boe-Bot, these values are down-sampled by a factor of 50 before being sent to the robot.

You might want to add some code to play a final parting tone and turn the LEDs off when the robot has finished its dance. See if you can figure this out yourself, and then check the sample code.

Now run your new service. It is not exactly musical. The LEGO NXT does a reasonable job of playing tones, but the Boe-Bot is not very good at it. The Boe-Bot speaker volume varies with the frequency, which is not something that you can control. However, you now have a way to warn people to get out of the way or for your robot to express its emotions.

First, you must know what type of sensors you are going to use so that you can include the appropriate reference in your project.

If you have created a new service, you should add a reference to RoboticsCommon.proxy. This contains definitions for the differential drive and for generic sensors like the contact sensor array. For other sensors, you will have to reference the appropriate DLL as explained below.

Once you have added a reference, insert a using statement at the top of your code with a short alias. This is not essential, but it saves a lot of typing.

Second, you need to know how the sensors are connected to the robot, i.e., to which ports they are attached. You probably won't be rebuilding your robot every day, so the assignment of sensors to ports on the robot will most likely remain fixed. The port assignments have to be entered into config files for the LEGO NXT V2, but for the Boe-Bot the pin connections are hard-coded into the services.

To read from a sensor, you must first subscribe to it. You do this by issuing a Subscribe request specifying both the name of the handler routine that you want called whenever a new update arrives, and which notification port to use. The handler receives information from the sensor inside the Body of an Update or Replace message (depending on the particular implementation).

When you subscribe to a sensor, it sends updates either on a regular basis or whenever the state of the sensor changes. In the case of a bump sensor, for example, you will only receive an update when the sensor is pressed or released. As long as it remains in the same state, there won't be any notification messages. A sonar sensor, however, tends to send updates fairly frequently because the distance it measures is constantly changing unless the robot is standing still, in which case you might not see any notifications.

Open the solution from ProMRDSChapter14WanderLegoV2. This solution is for the LEGO NXT using the V2 services.

If you look in the References, you will see NXTCommon and NXTBrick, as well as RoboticsCommon. The corresponding using statements appear at the top of WanderLegoV2.cs:

// References to the required services
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using sonarsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.SonarSensor.Proxy;
using touchsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.TouchSensor.Proxy;

Notice that the sonar sensor and the touch sensor are referenced explicitly. Because of this, the WanderLegoV2 service cannot be used with other types of robots. However, for most people this isn't a problem because they only have one robot!

The constants region contains a variety of different constants used in the code. The names and comments should be self-explanatory:

#region constants

const float drivePower = 0.5f;  // Power driving forward
const float rotatePower = 0.25f; // Power during rotation
const float veerPower = 0.6f;   // Power during veering away
const float rotateAngle = 45.0f; // Angle for Left turns
const int driveTime = 1200;     // Time to drive forward (millisec)
const int rotateTime = 920;     // Time to rotate

const float backupPower = 0.5f; // Power while backing up
const int backupTime = 1000;    // Time to back up on physical contact

#endregion

The connections to the service partners are made in the partners region. Notice that two ports are defined for each of the sensors—one to send requests to the sensor and the other for receiving notification messages containing updated sensor information. As usual, the partner creation policy is specified as UseExisting and the actual connections are established via the manifest and config files:

#region partners

// Partner: NxtDrive, Contract: http://schemas.microsoft.com/robotics/2006/05/drive.html
[Partner("NxtDrive", Contract = drive.Contract.Identifier, CreationPolicy =
PartnerCreationPolicy.UseExisting)]
drive.DriveOperations _nxtDrivePort = new drive.DriveOperations();

// Partner: NxtUltrasonicSensor, Contract:
http://schemas.microsoft.com/robotics/2007/07/lego/nxt/sonarsensor.html
[Partner("NxtUltrasonicSensor", Contract = sonarsensor.Contract.Identifier,
CreationPolicy = PartnerCreationPolicy.UseExisting)]
sonarsensor.UltrasonicSensorOperations _nxtUltrasonicSensorPort =
new sonarsensor.UltrasonicSensorOperations();

sonarsensor.UltrasonicSensorOperations _nxtUltrasonicSensorNotify =
new sonarsensor.UltrasonicSensorOperations();

// Partner: NxtTouchSensor, Contract:
http://schemas.microsoft.com/robotics/2007/07/lego/nxt/touchsensor.html
[Partner("NxtTouchSensor", Contract = touchsensor.Contract.Identifier,
CreationPolicy = PartnerCreationPolicy.UseExisting)]
touchsensor.TouchSensorOperations _nxtTouchSensorPort =
new touchsensor.TouchSensorOperations();
touchsensor.TouchSensorOperations _nxtTouchSensorNotify =
new touchsensor.TouchSensorOperations();

#endregion

You need to add some code to the Start method to set up the subscriptions to the sensors. Subscribe requests are made to both of the sensors, and the handlers are set up as well:

protected override void Start()
{
    base.Start();
    // Add service specific initialization here.

    // Subscribe to partners
    _nxtUltrasonicSensorPort.Subscribe(_nxtUltrasonicSensorNotify);
    _nxtTouchSensorPort.Subscribe(_nxtTouchSensorNotify);

    // Add notifications to the main interleave
    base.MainPortInterleave.CombineWith(
        new Interleave(
            new ExclusiveReceiverGroup(
               Arbiter.ReceiveWithIterator<sonarsensor.SonarSensorUpdate>(true,
_nxtUltrasonicSensorNotify, NxtUltrasonicSensorUpdateHandler),
               Arbiter.ReceiveWithIterator<touchsensor.TouchSensorUpdate>(true,
_nxtTouchSensorNotify, NxtTouchSensorUpdateHandler)
            ),
        new ConcurrentReceiverGroup()
        )
    );

}

Note that the handlers are added to the ExclusiveReceiverGroup, and this is combined with the existing handlers using MainPortInterleave.CombineWith.

The handler for the sonar updates receives a SonarSensorUpdate as its parameter. The current distance can be obtained from the message Body:

IEnumerator<ITask> NxtUltrasonicSensorUpdateHandler(sonarsensor.SonarSensorUpdate message)
{
    if (message.Body.Distance < 35)
    {
        LogInfo(LogGroups.Console, "Turn Left");

drive.RotateDegreesRequest request = new drive.RotateDegreesRequest();
        request.Power = rotatePower;
        request.Degrees = (double)rotateAngle;
        _nxtDrivePort.RotateDegrees(request);
    }
    else if (message.Body.Distance < 60)
    {
        LogInfo(LogGroups.Console, "Veer Left");
        drive.SetDrivePowerRequest powerRequest = new drive.SetDrivePowerRequest();
        powerRequest.RightWheelPower = veerPower;
        powerRequest.LeftWheelPower = veerPower/2;
        _nxtDrivePort.SetDrivePower(powerRequest);
    }
    else
    {
        drive.SetDrivePowerRequest powerRequest = new drive.SetDrivePowerRequest();
        powerRequest.RightWheelPower = drivePower;
        powerRequest.LeftWheelPower = drivePower;
        _nxtDrivePort.SetDrivePower(powerRequest);
    }
    yield break;

}

The algorithm is quite simple. When the robot gets too close to an obstacle, it turns to the left. If there is an obstacle farther away, it veers to the left (drives in an arc); otherwise, it drives straight ahead. When you run it, the robot tends to get quite close to obstacles before it reacts. This is dependent on how fast it is moving and how well it can see the obstacle. If the obstacle tends to absorb sound, then it is not as "visible" as a hard object that reflects sound well.

The last piece of code is a safety measure. The touch sensor detects a collision with an obstacle and backs the robot away:

IEnumerator<ITask> NxtTouchSensorUpdateHandler(touchsensor.TouchSensorUpdate message)
{
    if (message.Body.TouchSensorOn)
    {
        LogInfo(LogGroups.Console, "Bump!");

        // Back up to get away from the obstacle
        _nxtDrivePort.SetDrivePower(-backupPower, -backupPower);
        LogInfo(LogGroups.Console, ">>> Backing Up");
        yield return Arbiter.Receive(
            false,
            TimeoutPort(backupTime),
            delegate(DateTime timeout) { });
        _nxtDrivePort.SetDrivePower(0, 0);

    }

    yield break;
}

Messages are written to the console in various places in the code just so you can see what is happening. They slow down the running of the program, however, so when you are satisfied that your code is working, you should remove them or comment them out. The logging facility in MRDS enables you to set the trace level so that messages can be selectively filtered out.

Note that the reaction to a bump (when the touch sensor is pressed) involves moving backwards for a short period of time. During this time, the robot is not watching the sensors. However, it doesn't have any sensors facing backwards, so there is not much point.

If you run this program with the manifest and config files supplied, then the touch sensor should be plugged into sensor port 1 and the sonar sensor should be in port 4.

The robot will wander around trying to avoid obstacles. You can wave your hands in front of it and shepherd it around. You can also push the touch sensor if you want to see it react.

Open the solution located in ProMRDSChapter14WanderBoeBot. This solution is for the Parallax Boe-Bot using the updated Parallax services supplied on the book's website.

Have a look in the References. The only additional DLL is RoboticsCommon because this example only requires the generic Contact Sensor Array to work. The Parallax services implement a Contact Sensor Array using the two infrared sensors and the two whiskers.

if (num >= 0 && num < _state.bumperStates.Length)
    {
        _state.bumperStates[num] = notification.Body.Pressed;
        if (_state.bumperNames[num] == null)
            _state.bumperNames[num] = bumperName;
    }
    else
    {
        message = "Invalid Hardware ID: " + num.ToString();
        LogInfo(LogGroups.Console, message);
    }

// Handle the physical touch sensors first because these are the
    // most important
    if (_state.bumperStates[(int)BumperIDs.WHISKER_LEFT] || _state.bumperStates[(int)BumperIDs.WHISKER_RIGHT])
    {
        // Back up to get away from the obstacle
        _drivePort.SetDrivePower(-backupPower, -backupPower);
        LogInfo(LogGroups.Console, ">>> Backing Up");
        yield return Arbiter.Receive(
            false,
            TimeoutPort(backupTime),
            delegate(DateTime timeout) { });
        _drivePort.SetDrivePower(0, 0);

        if (_state.bumperStates[(int)BumperIDs.WHISKER_LEFT])
        {
            // Turn right
            LogInfo(LogGroups.Console, ">>> Turn Right");
            _drivePort.SetDrivePower(rotatePower, -rotatePower);
        }
        else
        {
            // Turn left
            LogInfo(LogGroups.Console, ">>> Turn Left");
            _drivePort.SetDrivePower(-rotatePower, rotatePower);
        }

// Wait a while
        yield return Arbiter.Receive(
            false,
            TimeoutPort(rotateTime),
            delegate(DateTime timeout) { });

        // Stop the motors and wait for robot to settle
        _drivePort.SetDrivePower(0, 0);

    }

// Now that everything has settled down
    if (_state.bumperStates[(int)BumperIDs.IR_LEFT] && _state.bumperStates[(int)BumperIDs.IR_RIGHT])
    {
        // Both IRs see something so back up
        // Eventually one of them must lose sight of the obstacle
        LogInfo(LogGroups.Console, ">>> Reverse");
        _drivePort.SetDrivePower(-drivePower, -drivePower);
    }
    else if (_state.bumperStates[(int)BumperIDs.IR_LEFT] && !_state.bumperStates[(int)BumperIDs.IR_RIGHT])
    {
        // Obstacle on the left, so turn right until gone
        LogInfo(LogGroups.Console, ">>> Veer Right");
        _drivePort.SetDrivePower(rotatePower, -rotatePower);
    }
    else if (!_state.bumperStates[(int)BumperIDs.IR_LEFT] && _state.bumperStates[(int)BumperIDs.IR_RIGHT])
    {
        // Obstacle on the right, so turn left until gone
        LogInfo(LogGroups.Console, ">>> Veer Left");
        _drivePort.SetDrivePower(-rotatePower, rotatePower);
    }
    else
    {
        // No obstacles so drive forwards
        LogInfo(LogGroups.Console, ">>> Forward");
        _drivePort.SetDrivePower(drivePower, drivePower);
    }
}