Задани на лабораторные работы. ПРК / Professional Microsoft Robotics Developer Studio

www.it-ebooks.info

Chapter 14: Remotely Controlling a Mobile Robot

</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>

Make sure that you save the manifest.

There is one last step before you can try out the program using a real LEGO NXT Tribot: Make sure that you have the appropriate config files. Otherwise, you will have to configure your Tribot yet again. You can get all of the config files from the ProMRDS\Config directory and copy them into your solution folder along with the manifest. Alternatively, you could copy your new manifest to the Config directory and run it from there.

Rebuild the Dance service just to be sure everything is okay, and then run the Dance service in the debugger again. Make sure that your LEGO NXT is turned on first and that you have a couple of meters of clear space around the robot. You don’t want it running into anything!

Using the Boe-Bot

If you have a Boe-Bot, then you need to create a different manifest. You can copy the necessary code from Parallax.BoeBot.manifest.xml. It has a similar pattern to the LEGO NXT, including a brick service (BASICStamp2) and a drive (BSDrive). Copy the service records that are highlighted below and paste

633

www.it-ebooks.info

Part IV: Robotics Hardware

them into Dance.manifest.xml in place of the LEGO code (if you did the previous section) or the Simulation Tutorial 2 (if you skipped the previous section):

<?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>

Save the manifest. Make sure that you copy the necessary config files from the ProMRDS\Config folder into your solution folder so that they are with the manifest. Run the Dance service in the Visual Studio debugger.

Managing Multiple Manifests

In the previous sections, you were told to copy the config files into your solution folder so that they can be found when the manifest is started. However, it is preferable to keep all of your config files and manifests together because this makes maintenance easier.

634

www.it-ebooks.info

Chapter 14: Remotely Controlling a Mobile Robot

Copy Dance.manifest.xml from the solution folder to ProMRDS\Config. Then, in Visual Studio, click Project Dance Properties. Select the Debug tab in the Properties window. In the command-line arguments, you should see the parameters that will be passed to DssHost.exe in order to start the Dance service. You need to change the manifest parameter as shown here:

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

Notice that the path points to the ProMRDS\Config folder (and there is no slash at the beginning). Because you have previously run the Tribot and/or Boe-Bot from manifests in this directory, appropriate config files are already in the directory.

Having to copy your manifest to ProMRDS/Config every time you change it is a little inconvenient if you are likely to change it often (although usually you would set it up and leave it alone). To avoid getting out of sync, you can add a pre-build command in the Project Properties in the Build Events tab. The command line to add is as follows:

copy “$(ProjectDir)dance.manifest.xml” “$(ProjectDir)..\..\Config\”

An alternative to constantly changing the manifest is to create different manifests for different robots. Five manifests are provided in the ProMRDS\Config directory:

Dance.BoeBot.manifest.xml

Dance.Lego.manifest.xml

Dance.LegoV2.manifest.xml

Dance.SimTut2.manifest.xml

Dance.Simulation.manifest.xml

In addition, there is a batch file in the MRDS bin folder called Dance.cmd that can be used to run any of these five manifests using a command-line parameter. This might seem a little excessive, but it is intended to illustrate different ways to handle manifests.

Reviewing the Results

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.

635

www.it-ebooks.info

Part IV: Robotics Hardware

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 180degree 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.

Improving the Behavior

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:

The timers under Windows are not very reliable, and the CCR doesn’t really improve on them. If the PC suddenly performs garbage collection or flushes the disk cache, then a timer might run for longer than you requested.

636

www.it-ebooks.info

Chapter 14: Remotely Controlling a Mobile Robot

Motor speed varies depending on the battery level and it is not linear. For very short time periods, the motor might not have enough time to get up to full speed. Conversely, once you reach a certain power level the motor is rotating at its top speed, so increasing the power will have no effect.

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.

The RotateDegrees and DriveDistance methods are not implemented for the LEGO NXT, and they throw an exception that will crash your program. At least you will know they are not implemented! However, the newer LEGO NXT V2 services do implement RotateDegrees and DriveDistance using the wheel encoders. You can actually see the encoders in operation if you view the Drive service in a web browser (refer to Figure 14-9).

The Parallax BoeBot services that come with MRDS do not implement RotateDegrees or DriveDistance either, but two implementations are provided in Chapter 16 and are included in the Parallax BoeBot services from the website for this book (www.proMRDS.com. or www.wrox.com).

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

1. Insert a using statement at the top of the code:

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

// Added for the Fault class

using W3C.Soap;

The using W3C.Soap is not essential, but it introduces the Fault class, which can be returned if there is an error, or if a DriveDistance or RotateDegrees request is interrupted by another request (and therefore cancelled).

2. In the Constants region, add the following values:

// Values for “exact” movements using DriveDistance and RotateDegrees

637

www.it-ebooks.info

Part IV: Robotics Hardware

The purpose of these constants is as follows:

controlledMoves determines whether DriveDistance and RotateDegrees are called or the simple approach of using timed moves is used instead.

waitForMoves indicates whether the code should wait for DriveDistance and RotateDegrees to send a response message indicating that the motion is complete.

driveDistance is the actual distance to drive forwards expressed in meters.

rotateAngle is the amount to rotate by (with positive values being to the left).

3. Now find the Behavior method and replace the code inside the innermost for loop as shown in the following code. The first section of code uses the DriveDistance and RotateDegrees requests, but calls them inside an Arbiter.Choice in order to wait for the response message that is sent when the motion completes. This means that timers are not used. Note that these requests might return a Fault instead of the default response. The error handling isn’t good but it illustrates how a local variable, called success, can be accessed inside a delegate:

// Drive along the four sides of a square for (int side = 0; side < 4; side++)

{

LogInfo(LogGroups.Console, “Side “ + side);

if (controlledMoves && waitForMoves)

{

bool success = true;

//Drive straight ahead yield return Arbiter.Choice(

_drivePort.DriveDistance(driveDistance, driveSpeed), delegate(DefaultUpdateResponseType response) { }, delegate(Fault f) { success = false; }

);

//Wait for settling time

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

// Now turn left

yield return Arbiter.Choice( _drivePort.RotateDegrees(rotateAngle, rotateSpeed), delegate(DefaultUpdateResponseType response) { }, delegate(Fault f) { success = false; }

);

if (!success)

LogError(“Error occurred while attempting to drive robot”);

}

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

638

www.it-ebooks.info

Chapter 14: Remotely Controlling a Mobile Robot

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

639

www.it-ebooks.info

Part IV: Robotics Hardware

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.

Flashing and Beeping

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:

1. Open the Dance service in Visual Studio. You need to add a reference (see Chapter 3) to the appropriate brick. Depending on the robot that you have, you will need either of the following:

nxtbrick.y07.m07.proxy (for LEGO NXT V2) BASICStamp2.Y07.M06.Proxy (for Boe-Bot)

It does no harm to add references to both of these, but it is clearly not necessary if you only have one of these robots. The sample code has both references.

2. Once you have added the references, insert the corresponding using statement at the top of Dance.cs. The two statements are shown here:

using legonxt = Microsoft.Robotics.Services.Sample.Lego.Nxt.Brick.Proxy;

using stamp = Microsoft.Robotics.Services.BasicStamp2.Proxy;

The LEGO NXT V2 services have departed from the previous pattern and are located under the Sample branch of the namespace. Also notice that each of them uses an alias to make it easier to reference methods and properties.

3. Locate the region called partners and expand it. The Dance service needs to partner with the correct Brick service. Because the code will be specific to the particular brick, it doesn’t make sense to put the partnership in the manifest. At this point you start to lose the hardware independence.

If you are using a LEGO NXT, then insert the following code:

// Add a LEGO NXT V2 partner to allow direct access to its methods [Partner(“LegoNxtV2”, Contract = legonxt.Contract.Identifier,

CreationPolicy = PartnerCreationPolicy.UseExisting)]

legonxt.NxtBrickOperations _legonxtPort = new legonxt.NxtBrickOperations();

If you are using a Boe-Bot, insert this code instead:

640

www.it-ebooks.info

Chapter 14: Remotely Controlling a Mobile Robot

// Add a BASICStamp2 partner to allow direct access to its methods [Partner(“Stamp”, Contract = stamp.Contract.Identifier, CreationPolicy =

PartnerCreationPolicy.UseExisting)]

stamp.BasicStamp2Operations _stampPort = new stamp.BasicStamp2Operations();

Do not add both of these to the service at the same time! It makes no sense to partner with two different bricks (unless you want to control two robots simultaneously). If you check the code supplied on the book’s website (www.proMRDS.com or www.wrox.com), you will find that conditional compilation has been used to enable the same source code to be used for the LEGO NXT, the Boe-Bot, and Simulation. This conditional code is omitted here for clarity.

4. The PartnerCreationPolicy for the brick is set to UseExisting. This is important because your manifest will be creating the brick anyway, and you don’t want the service to create a second instance of the Brick service, which would happen if you used a different policy.

When a service partner is specified this way, dsshost will try to establish the partnership for a short period of time before giving up. This enables all of the services enough time to start up and add themselves to the service directory. The code also creates a new port that can be used for sending requests to the Brick service (and receiving responses).

5. With the partnership established, you can call the appropriate brick method to play a tone:

For the LEGO NXT V2, the following code should be added at the top of the innermost for loop in the Behavior method:

for (int side = 0; side < 4; side++)

{

LogInfo(LogGroups.Console, “Side “ + side);

// Make a beep (no lights to flash on a LEGO brick!)

_legonxtPort.PlayTone(3500 + side * 250, 50);

The PlayTone request takes a frequency (in hertz) and a duration (in milliseconds) as its parameters. This causes the LEGO NXT brick to “beep.” Notice that the frequency increases for each side of the square just to make it a little more interesting.

For the Boe-Bot, there is a little more code:

for (int side = 0; side < 4; side++)

{

LogInfo(LogGroups.Console, “Side “ + side);

byte op1, op2;

// Toggle the lights to make it pretty and beep as well :-)

op1 = ((side & 1) != 0) ? (byte)stamp.PinOperations.OUTPUT_HIGH : (byte)stamp.PinOperations.OUTPUT_LOW;

op2 = ((side & 2) != 0) ? (byte)stamp.PinOperations.OUTPUT_HIGH :

(byte)stamp.PinOperations.OUTPUT_LOW; // Pins 14 and 15 are LEDs

_stampPort.SetPins(14, op1, 15, op2);

//The speaker is on pin 4, but we don’t need to know this because

//the firmware does the tone

_stampPort.PlayTone(3500 + side * 250, 50);

641

www.it-ebooks.info

Part IV: Robotics Hardware

The code for the Parallax Boe-Bot on the book’s website has been enhanced so that it supports some additional functions. The SetPins and PlayTone requests are two of these functions. You must use the Parallax services from the book’s website (www.proMRDS.com or www.wrox.com)in order for this code to work. It will not work with the MRDS V1.5 Parallax services.

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.

Be careful if you update the pins directly because all of them except pin 11 have specific purposes. In particular, pins 0, 1, 5, and 6 are connected to the eb500 Bluetooth module and you should leave them alone!

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.

642