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

www.it-ebooks.info

Chapter 14: Remotely Controlling a Mobile Robot

Although you might enjoy it, your office co-workers might get annoyed with the constant beeping of your robot. Be thoughtful in your coding.

Exploring Using Sensors

Although you now have a working service that can exercise your robot, the robot is still effectively blind. To make it interact with its environment, you need to add some code that uses the sensors.

Because the sensors on the LEGO NXT Tribot and the Parallax Boe-Bot are quite different, this section develops two separate services. You should read through both of them because they illustrate different aspects of MRDS.

You built a service from scratch at the beginning of this chapter, so to save time, this section steps you through code supplied on the book’s website. You can create a new service if you want the practice, but it isn’t necessary. These services can be found in the ProMRDS\Chapter14 folder and are called

WanderLegoV2 and WanderBoeBot.

Reading from the Sensors

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.

643

www.it-ebooks.info

Part IV: Robotics Hardware

Making a LEGO NXT Wander Around

Open the solution from ProMRDS\Chapter14\WanderLegoV2. 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:

#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();

644

www.it-ebooks.info

Chapter 14: Remotely Controlling a Mobile Robot

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”);

(continued)

645

www.it-ebooks.info

Part IV: Robotics Hardware

(continued)

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;

}

646

www.it-ebooks.info

Chapter 14: Remotely Controlling a Mobile Robot

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.

Making a Boe-Bot Wander Around

Open the solution located in ProMRDS\Chapter14\WanderBoeBot. This solution is for the Parallax BoeBot 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.

It is possible to use the Contact Sensor Array with the LEGO NXT as well. However, the NXT services attempt to use every possible sensor as a bumper, including the ultrasonic sonar sensor, sound sensor, and light sensor. Under the V2 services, the sonar sensor is further divided into virtual “near” and “far” bumpers. This makes dealing with the NXT Contact Sensor Array much more complicated. Furthermore, the sonar bumpers do not always issue release messages, which makes the logic very convoluted.

1. The following using statements are at the top of the file:

//Add a reference to a generic Bumper (in Robotics.Common.Proxy) using bumper = Microsoft.Robotics.Services.ContactSensor.Proxy;

//Add a reference to the Differential Drive

using drive = Microsoft.Robotics.Services.Drive.Proxy; // Required for Fault class

using W3C.Soap;

2. As usual, there is a constants region:

#endregion

647

www.it-ebooks.info

Part IV: Robotics Hardware

3. By now you should be familiar with setting up partners. Notice that there are two bumper ports:

#region partners

//Partner with a Bumper (Contact Sensor) service [Partner(“bumper”, Contract = bumper.Contract.Identifier,

CreationPolicy = PartnerCreationPolicy.UseExisting)]

//Create a port to send requests

private bumper.ContactSensorArrayOperations _bumperPort = new bumper.ContactSensorArrayOperations();

// Create the bumper notification port

private bumper.ContactSensorArrayOperations _bumperNotificationPort = new bumper.ContactSensorArrayOperations();

//Partner with a Differential Drive service [Partner(“drive”, Contract = drive.Contract.Identifier,

CreationPolicy = PartnerCreationPolicy.UseExisting)]

//Create a port to talk to the diff drive drive.DriveOperations _drivePort = new drive.DriveOperations();

#endregion

4. The Start method is modified so that it subscribes to the contact sensor array:

protected override void Start()

{

base.Start();

//Add service specific initialization here.

//Allocate the necessary storage for the bumper state

//The size of this array will depend on the particular robot _state.bumperStates = new bool[4];

_state.bumperNames = new string[4]; for (int i = 0; i < 4; i++)

{

_state.bumperStates[i] = false; _state.bumperNames[i] = null;

}

//Now subscribe to bumper notifications SpawnIterator(SubscribeToBumpers);

SpawnIterator(Countdown);

}

This example takes a different approach to handling the bumpers. Additional fields are added to the service state to remember the latest logic states of the various bumpers, including their names. This makes the values visible in a web browser via the service directory. However, this is not the reason for adding them to the state. When you receive a notification from the Contact Sensor Array, it is for a single sensor. The logic for wandering around needs to keep track of the current state of all of the bumpers.

648

www.it-ebooks.info

Chapter 14: Remotely Controlling a Mobile Robot

5. The additions to the state are made in WanderBoeBotTypes.cs.

[DataContract()]

public class WanderState

{

[DataMember]

public bool[] bumperStates;

[DataMember]

public string[] bumperNames;

}

Notice that the new properties are marked with the [DataMember]attribute and declared as public. This is important for making them visible outside the service.

6. During initialization, a subscription is made to the bumpers. This is done in an iterator, although it does not have to be:

IEnumerator<ITask> SubscribeToBumpers()

{

//Subscribe to the bumper service

//Receive notifications on the bumperNotificationPort _bumperPort.Subscribe(_bumperNotificationPort);

//Start listening for updates from the bumper service

//Note that we use the Exclusive Receiver Group so that

//two bumper messages cannot be processed at the same time Activate(

Arbiter.Interleave(

new TeardownReceiverGroup(), new ExclusiveReceiverGroup(

Arbiter.ReceiveWithIterator<bumper.Update>

(true, _bumperNotificationPort, BumperHandler)

),

new ConcurrentReceiverGroup()

)

);

}

7. To finish off the initialization, the Countdown method is called. This delays starting the robot for 10 seconds. In this case it does have to be an iterator because it uses yield return to wait for timeouts:

IEnumerator<ITask> Countdown()

{

//Wait for the robot to initialize, otherwise it will

//miss the initial command

for (int i = 10; i > 0; i--)

{

LogInfo(LogGroups.Console, i.ToString()); yield return Arbiter.Receive(

false,

TimeoutPort(1000), delegate(DateTime timeout) { });

}

(continued)

649

www.it-ebooks.info

Part IV: Robotics Hardware

(continued)

LogInfo(LogGroups.Console, “Starting now ...”);

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

//Start the robot on its way!

_drivePort.SetDrivePower(drivePower, drivePower);

}

8. The drive must be enabled before sending commands to it, and finally the robot sets off driving forwards.

9. The code that handles the bumper notifications needs to know which bumper is which. Although it is possible to look at the names of the contact sensors, they will never change so an enum is used:

private enum BumperIDs { IR_LEFT = 0, IR_RIGHT, WHISKER_LEFT, WHISKER_RIGHT };

10. Finally, the bumper handler is responsible for adjusting the motor power based on the combination of active bumpers. The first part of the code simply displays a message indicating which bumper has changed and its state:

IEnumerator<ITask> BumperHandler(bumper.Update notification)

{

string message; string bumperName;

//Find out which bumper this is

//BoeBot numbers from 1 to 4

int num = notification.Body.HardwareIdentifier - 1;

if (string.IsNullOrEmpty(notification.Body.Name)) bumperName = “NO NAME”;

else

bumperName = notification.Body.Name.ToLowerInvariant();

if (!notification.Body.Pressed)

{

message = “Bumper “ + num.ToString() + “ (“ + bumperName + “) was released.”;

LogInfo(LogGroups.Console, message);

}

else

{

message = “Bumper “ + num.ToString() + “ (“ + bumperName + “) was pressed.”;

LogInfo(LogGroups.Console, message);

}

650

www.it-ebooks.info

Chapter 14: Remotely Controlling a Mobile Robot

Processing Bump Notifications

If the hardware identifier is between 1 and 4, then the new state of the bumper is recorded, including the name if it has not been seen before:

The LEGO NXT V2 issues release messages for each bumper initially. The Parallax services do not do this.

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);

}

The whiskers are physical contact sensors. If one of them is triggered, then the robot has either collided with an object or it is going to very soon! Therefore, the whiskers are checked first. If either of them is depressed, then the robot backs up for a little while and then turns to the left or right, depending on which of the whiskers was depressed:

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

}

651

www.it-ebooks.info

Part IV: Robotics Hardware

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

}

Next, the code checks the infrared sensors. If both of them see an obstacle, then the robot backs up. However, if only one of them can see something, then the robot turns in the appropriate direction. If no obstacles are visible, it simply charges straight ahead:

// Now that everything has settled down

if (_state.bumperStates[(int)BumperIDs.IR_LEFT] && _state.bumperStates[(int)Bum perIDs.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[(i nt)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);

}

}

That completes this section on wandering around. Try out the code if you have a Boe-Bot. It is more fun to play with than the LEGO NXT.

652