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

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

Notice that ClearPendingStop sends a Fault to the original caller to indicate that the motion was interrupted. The response port is in a global variable. You cannot put it in the timer message because then it would be inaccessible.

If there are no interruptions, then the timer eventually fires and the StopMotionHandler is executed:

//Stop Motion Handler

//Called in response to a Stop message when a timed motion finishes public virtual IEnumerator<ITask> StopMotionHandler(StopMotion stop)

{

//Somebody beat us to it!

if (!_ackPending) yield break;

//Another request has arrived and overridden this one

//so there is still one pending, but it is not us

if (stop.Body.AckNumber != _ackCounter) yield break;

// Now stop the robot and post a response back to say the move

is complete

drive.SetDrivePower power = new drive.SetDrivePower();

//Set the power power.Body.LeftWheelPower = 0; power.Body.RightWheelPower = 0;

//Set the response port power.ResponsePort = _ackPort;

//Use the internal handler for throttling _internalDrivePowerPort.Post(power); _state.DriveState = drive.DriveState.Stopped;

//Clear the ACK

_ackPending = false; _ackPort = null;

}

This handler posts a Stop request to the internal drive port and sets the ResponsePort in this message to the response port that was provided in the original message. When the internal handler executes, it posts a response back to the original caller and everyone is happy.

This is a complicated piece of code. You might want to read over it again, or step through it in the debugger. However, once you have figured out a mechanism like this and verified that it works, you can use it repeatedly.

783

www.it-ebooks.info

Part IV: Robotics Hardware

Using XSLT

Although Microsoft supplies XSLT files with the source code for many different services, there are no generic XSLT files included with the generic contracts. The generic Differential Drive has a standard state data type, so an XSLT file can be used with any implementation of a drive service. You can find the IntegratorDrive.xslt file in the Resources folder in the Integrator solution.

The first few lines of the file define the data types (contracts) that are used to interpret the XML data from the HttpGet operation. Notice that these are all generic contacts:

<?xml version=”1.0” encoding=”utf-8”?> <xsl:stylesheet version=”1.0”

xmlns:xsl=”http://www.w3.org/1999/XSL/Transform” xmlns:s=”http://www.w3.org/2003/05/soap-envelope”

xmlns:drive=”http://schemas.microsoft.com/robotics/2006/05/drive.html”

xmlns:motor=”http://schemas.microsoft.com/robotics/2006/05/motor.html” xmlns:physical=”http://schemas.microsoft.com/robotics/2006/07/physicalmodel.html”>

If you open this XSLT file in Visual Studio you might see warning messages that it could not find the schemas. Ignore these warnings. They do not affect compilation and will go away when you close the file.

This file uses the standard Microsoft MasterPage:

<xsl:import href=”/resources/dss/Microsoft.Dss.Runtime.Home.MasterPage.xslt” />

It also includes a small image in the page heading to make it look more professional. This PNG file is an embedded resource that is also in the Resources folder:

<xsl:template match=”/”> <xsl:call-template name=”MasterPage”>

<xsl:with-param name=”serviceName”> <img

src=”/resources/Integrator.Y2008.M01/ProMRDS.Robotics.Integrator.Resources. Integrator.Image.png” align=”middle”/> Integrator Generic Drive

</xsl:with-param>

<xsl:with-param name=”description”> Provides access to the Integrator Drive (Uses the Generic Drive contract)

</xsl:with-param> </xsl:call-template>

</xsl:template>

Everything in the remainder of the file refers to fields in the Differential Drive state using generic data types. It is just a large table, so there is no point in reproducing the code here. The resulting output is shown in Figure 17-5.

784

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

Figure 17-5

Notice that the current power is 0.3 for both wheels. Anything above 0.25 turns the motors on. Below this they do not move. In addition, the Drive State is DriveDistance, which is the command that is currently being executed.

Testing a Generic Brick Robot

Once you have built a robot based on the Generic Brick contract, you need to test it. A lot of operations are defined (19 in fact) for the generic brick. This is a lot to test, although many operations can be tested simply by driving the robot around using TeleOperation or the Dashboard and viewing its state in a web browser.

To make life easier for you, there is a test service for the generic brick. It is in the folder ProMSRS\ Chapter17\GenericBrickTest. To use this service with a robot, you must edit the manifest and insert the necessary code to start your robot. The sample manifest that is provided has the code for the Integrator and the Hemisson. Just comment or delete the one you don’t want.

785

www.it-ebooks.info

Part IV: Robotics Hardware

Because the robot is based on the Generic Brick contract, the test service can partner with a generic brick and it does not have to know what type of robot it is. The service is not very sophisticated, but it can exercise all of the operations, including Drop! It uses console output to display a list of operations by number (see Figure 17-8 later in the chapter). In some cases an operation requires further information, which you are prompted to enter.

If you select an operation that is not implemented (such as DriveDistance or RotateDegrees), the result will depend on whether you are running the test service with the debugger or not. The default behavior for operations that are not implemented is to throw an exception.

When the debugger is running, it sees an unhandled exception and breaks execution at that point in the code. Normally you would not continue from an exception, but in the DSS environment it is safe to do so. Just continue running the program and DSS will convert the exception to a Fault and send a response back. If you are running the test service without the debugger, then the Fault is generated automatically.

The test service sets a timeout on all operations so that it does not hang if the implementation has a problem or you forgot to post back a response in your handler code. One possible problem is that you can easily create a deadlock inside an operation. For example, if you call another operation inside a handler and wait for a response, this is a recipe for deadlock. If the handler is Exclusive, no other handlers can run until it completes, so you cannot call any other operations. Even if it is Concurrent, if you make a request to an Exclusive handler, then it cannot run until the Concurrent one completes. Calling a Concurrent operation from inside a Concurrent handler is okay though.

Although the test service has some limited “knowledge” of what the operations do, most of this knowledge is involved in displaying the results. It should be possible to create a more sophisticated service that displays a graphic depiction of the robot (based on the locations of the sensors and actuators), but this is left as another exercise for you.

This XSLT file can be used for any Differential Drive service with only minor changes, i.e., the title at the top of the page and the icon.

The Hemisson Robot

Hemisson robots are manufactured by K-Team (www.k-team.com), which is better known in the research field for its Khepera robots. The Hemisson is designed to be a much cheaper alternative to the Khepera for the educational market.

Figure 17-6 shows a Hemisson loaded up with a Bluetooth dongle and a wireless camera, plus their batteries. This is a heavy load for the poor little fellow, which does not have very powerful motors and only tiny plastic wheels.

786

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

Figure 17-6

The Sena Parani-SD100 Bluetooth-to-serial converter runs off a 9V battery. The DIP switches are configured for 115200 baud and no hardware flow control. Using the ParaniWin software, the SD100 is placed into Mode 3 (accept connections from any other Bluetooth device). It must be connected to the Hemisson using a null modem connector, which also changes the gender of the serial port. This isn’t visible in Figure 17-6 because it is behind the batteries. It is no bigger than two back-to-back 9-pin serial connectors and actually comes in the full Sena SD100 kit.

The Swann wireless MicroCam in Figure 17-6 is stuck in a lump of BluTac. The camera is unplugged from its 9V battery because it does not have a power switch, which is why there is a plug dangling at the front of the Hemisson. Bluetooth interferes with the video signal to some extent, but the reverse does not seem to be the case in this configuration, and the Hemisson can drive around quite reliably with the camera running.

Hemisson Brick Service

Because it has LEDs, switches, and analog range sensors, the Hemisson is a better example of the generic brick than the Integrator. The code for the Hemisson services is substantially the same as for the Integrator, but there are some points worth mentioning:

The Hemisson comes with an onboard monitor program already loaded. For the purpose of the exercise, the authors chose not to modify this code and instead work with the existing command set. You can download replacement firmware if you want to, but using the existing firmware enables other people to use the services without the hassle of downloading different firmware.

787

www.it-ebooks.info

Part IV: Robotics Hardware

The Hemisson requires a pause of at least 1ms between characters, and up to 100ms between commands. It has a particular problem that the monitor program will hang if a data overrun occurs, i.e., if you send a command before it has finished processing the previous one, then the robot locks up and you have to turn it off and on again to reset it. This appears to be a bug in the firmware, but K-Team support could not shed any light on the matter.

The Hemisson has four LEDs. One of these (the On/Off LED) flashes constantly when the Hemisson is running in program mode, so setting it has no effect because it is immediately overridden. (If you watch carefully, it does turn on solidly for a very brief period and then it starts flashing again.) However, the service allows you to set all four. That leaves three LEDs that can be used to indicate status — two yellow ones at the front and one red one at the back right.

There is a set of four switches on the Hemisson. However, these must all be in the “up” position when the robot is turned on so that it enters “program” mode. The other positions initiate different behaviors such as autonomous wandering and line following. Once the power is on, you can change the switches and then start the MRDS service. Therefore, you can use the switches to select different functions within your own service.

The switch values are obtained during polling as part of the fast binary read (explained under “IR Sensors”), but notifications are not sent for changes in the switch states.

The switches are not polled in the current service implementation — they are only read when the service starts. This was a deliberate design decision because the switches are rarely used, especially while the robot is moving around, and the extra overhead of sending notifications is not warranted. Reading them would slow down the polling of the IR sensors, which are much more important. You can, however, issue an explicit GetSwitches request if you want to get the settings.

IR Sensors

There are eight infrared range sensors on the Hemisson, although two of them are at the front facing downward and are intended for line following. (The generic brick DeviceFunctions enum has a function explicitly for line following.) To read these using the standard command involves quite a long message because the protocol is designed to be human-readable.

There are new, undocumented commands in the latest version of the Hemisson firmware (V1.51) called “fast binary read” and “fast binary write” that transmit the data in binary. These can save a considerable amount of time because the response packet is shorter than it would be for the ASCII version. If you want more information, download the source code for the HemiOS and read through it to see how the commands work.

The IR sensors have a very limited range and are highly nonlinear. The Hemisson documentation includes a graph showing the response of the IR sensors. Very rough testing showed that they do not register anything until the distance is about 3.5cm or less. Then the values climb rapidly.

The response curves for some of the IR sensors are shown in Figure 17-7. (The Excel spreadsheet is in the solution folder and is called Calibration-IRRange.xls.) Note that measuring distances from the sensors is very difficult, and when you get close to the sensors, even a difference of 1 millimeter can make a substantial difference to the reading. Therefore, this graph is provided for illustration purposes only — it is not intended to be definitive.

788

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

Hemisson IR Range Readings

Figure 17-7

The main point to note about Figure 17-7 is that if you drive the robot too fast, it might not sense obstacles in time to avoid them.

Because there are so many range sensors, a contact sensors array (also called bumpers) has been implemented for the Hemisson. (This wasn’t done with the Integrator because it only has a single digital input for an obstacle sensor.) This uses the MRDS generic contract for contact sensors. The service was created in a similar way to the Drive service for the Integrator, i.e., DssNewService was used to create a new service and then the source files were moved to the existing Hemisson solution.

The bumper service subscribes to the brick to receive updates from the IR sensors. The code to implement the bumper service is almost trivial. The most important operation is the one that receives the sensor update notifications from the brick:

///<summary>

///Receive infrared notifications from the Hemisson service

///</summary>

///<param name=”notification”></param>

private void UpdateSensorsHandler(brick.UpdateSensors notification)

{

int i;

bool changed; bool found; bool newState;

foreach (bumper.ContactSensor b in _state.Sensors)

{

(continued)

789

www.it-ebooks.info

Part IV: Robotics Hardware

(continued)

{

if (dev.HardwareIdentifer == b.HardwareIdentifier)

{

found = true;

//Check on the purpose of the device

//NOTE: IR sensors are NOT marked as Bumpers!

//A Bumper is a physical switch, or an IR sensor that only

//has a Digital output. The Hemisson IR sensors are range

//sensors, except for the Line Followers.

if (dev.Function == brick.DeviceFunctions.LineFollower) newState = LineDetected((int)dev.Value);

else

newState = BumperPressed((int)dev.Value); if (newState != b.Pressed)

{

changed = true; b.Pressed = newState;

b.TimeStamp = DateTime.Now; break;

}

}

}

if (!found)

LogError(LogGroups.Console, “IR sensor not found in update”);

if (changed) this.SendNotification<bumper.Update>(_subMgrPort, new

bumper.Update(b));

}

}

Notice that bumper notifications are sent only when a bumper state changes, so here you have a notification message triggering yet another notification message, possibly to different subscribers.

The BumperPressed function applies a threshold to determine whether the bumpers have been triggered. Looking at Figure 17-7, a value of 10 corresponds to about 3cm, so this is used to trigger the obstacle sensors. (The noise in the IR measurements is less than 10 units, so this is not a problem.)

For the line-following sensors, the behavior of the two sensors is not consistent. This means that applying a single threshold (in the routine LineDetected) to both of them does not give optimum results. Unfortunately, the Contact Sensors Array contract does not take into account “virtual” bumpers, so there is nowhere to store a threshold value on a per-sensor basis. If you have a Hemisson, you will want to test it yourself and perhaps change the threshold settings, and maybe even provide an array of thresholds based on the hardware IDs.

IR sensor updates occur at about 10Hz. Occasionally you will see an error message in the console window because a poll times out. The inter-command delay is squeezed as low as possible (currently 50ms),

but perhaps it needs to be increased.

790

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

Testing the Hemisson Brick

Because the Hemisson has several devices, it is more important to have a test program than it was for the Integrator because there are more operations to test. For example, Figure 17-8 shows the GenericBrickTest service being used to test the SetLEDs operation on a Hemisson.

Figure 17-8

You can also test subscriptions using the test service, although it only displays the contents of the first notification message. To use the ConfigureBrick, SetActuators, and UpdateSensors operations, you need to first do a Get. The service then uses the data from the Get in these other operations. This is necessary because the service has no understanding of what the data means, and if it simply generated arbitrary outputs, there is no telling what the robot might do!

While you are using the test service, you should also have a web browser open and use it to view the state information for the various services — Robot Brick, Drive, and Bumpers.

Hemisson Drive Service

The Hemisson has nine different drive power settings. At the lowest settings, 1 and 2, the robot cannot move with all of the additional equipment loaded on it because the motors are not very strong.

To implement timed motions, it is necessary to map each motor power setting to a time factor for calculating the elapsed time for a given distance or rotation angle. This is a straightforward, but timeconsuming, process. The steps are as follows:

791

www.it-ebooks.info

Part IV: Robotics Hardware

3. The recorded distances can then be plotted using Excel. Figure 17-9 shows the graph of distance traveled versus power setting. (The spreadsheet is included in the Hemisson folder and is called

Calibration-Drive.xls.)

Hemisson Drive Calibration

You might wonder why there is a flat spot between 0.5 and 0.6. This is because the range for drive power is a double from 0 to 1.0, which TeleOperation expresses as 0 to 1000, but the Hemisson only has power settings of 0 to 9. When remapping the power to Hemisson settings, 0.5 and 0.6 both map to the same power level. It might be more sensible to change the mapping and throw away anything lower than 100. You can investigate this if you’re interested.

Apart from these problems, the curve is fairly straight. In the real world, of course, there is no such thing as a linear system. Therefore, Excel is used to calculate the required time factor for each of the power settings so that the robot will move the correct distance, which is 30cm in this experiment. These values are then placed in an array, rather than trying to obtain an equation for a straight line.

When a DriveDistance request is processed, the time factor is looked up and multiplied by the given distance to obtain the delay time in milliseconds. The code to do this is as follows:

//Experimentally determined time factors to travel a specified distance

//Then adjusted slightly after retesting

private static int[] TimeFactors =

{

792