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

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

{

// Construct a mask for this Pin

int mask = 1 << _state.Sensors[i].Pin; bool oldState = _state.Sensors[i].State;

//Check if it is High or Low if ((n & mask) != 0)

{

_state.Sensors[i].State = true; _state.Sensors[i].Value = 1;

}

else

{

_state.Sensors[i].State = false; _state.Sensors[i].Value = 0;

}

//Check for a state change

if (_state.Sensors[i].State != oldState)

{

//If it is the IR bumper, toggle an LED

//This is for debugging ...

if (_state.Sensors[i].Pin == (int)Pins.IrBumper)

{

_state.LEDs = util.UpdateBitmask( _state.LEDs, LEDIds, (int)HwIds.LED2, _state.Sensors[i].State);

_control.SetPin((int)Pins.LED2,

_state.Sensors[i].State);

}

changed = true;

}

}

}

// Only send notifications on changes if (changed)

SendNotification<brick.UpdateSensors>(_subMgrPort, new brick.UpdateSensorsRequest(_state.Sensors));

}

}

// Finally, set the timer again SetPollTimer();

}

This routine updates the sensor state. It makes use of some of the helper functions that are defined in the GenericBrickUtil DLL prefixed with “util.” For debugging purposes, LED 2 reflects the current status of the IR bumper. This is not necessary, but it is an easy way to verify that polling is working. The code also checks to see if any of the values have changed; if so, it sends a notification to all subscribers with the new sensor information. Notice that it does not send the entire state. Before it finishes, the routine kicks off a new timer so that it will be called again.

773

www.it-ebooks.info

Part IV: Robotics Hardware

Completing the Brick Operations

Now that the infrastructure is in place, you can go back to the operation handler stubs that were automatically created as part of the new service. Filling these in should be straightforward, so they are not covered here.

As an example, consider the PlayTone operation. It has to make the buzzer sound on the robot. The Integrator cannot control the frequency of the sound, although some robots can do this. The duration is controlled by the service turning the buzzer on and then off again after the appropriate time delay:

///<summary>

///PlayTone Handler

///</summary>

///<param name=”update”></param>

///<returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)]

public virtual IEnumerator<ITask> PlayToneHandler(brick.PlayTone update)

{

//Turn on the buzzer and don’t bother to wait for a response util.UpdateDeviceStateInList(_state.Actuators, (int)HwIds.Buzzer,

true);

_control.SetPin((int)Pins.Buzzer, true);

// Set up a timer to turn the buzzer off Activate(Arbiter.Receive(false, TimeoutPort(update.Body.Duration),

delegate(DateTime dt)

{

// Turn off the buzzer util.UpdateDeviceStateInList(_state.Actuators,

(int)HwIds.Buzzer, false);

_control.SetPin((int)Pins.Buzzer, false);

}

));

// Send back the default response update.ResponsePort.Post(DefaultUpdateResponseType.Instance);

yield break;

}

This function uses the SetPin routine mentioned earlier to turn on the appropriate pin. Notice that the current state is also updated to reflect this change. However, actuator changes do not result in notifications.

Then a timeout is set with a delegate that turns the buzzer off (and updates the state again). A response is sent back before the routine finishes. Note that this response is sent back before the timeout has completed because although a receiver is activated, the code does not wait for it. There is no point to keeping the caller waiting.

As a somewhat different example, look at the GetSensors handler. Due to the design, it is possible to request any combination of sensors that you like. However, this makes the processing a little more complicated because you have to look up each device. If an invalid hardware identifier is encountered in the requested device list, then the operation throws an exception:

774

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

///<summary>

///GetSensors Handler

///</summary>

///<param name=”query”></param>

///<returns></returns> [ServiceHandler(ServiceHandlerBehavior.Concurrent)]

public virtual IEnumerator<ITask> GetSensorsHandler(brick.GetSensors query)

{

int n;

// Create a new result list (which might end up empty!) List<brick.Device> list = new List<brick.Device>();

foreach (brick.Device d in query.Body.Inputs)

{

// Look in the State for a matching Hardware Id

n = util.FindDeviceById(_state.Sensors, d.HardwareIdentifer); if (n < 0)

{

// No such device! Throw an exception and give up!

throw new ArgumentOutOfRangeException(“No such hardware identifier: “ + d.HardwareIdentifer);

}

else

{

// Add the device to the result list list.Add((brick.Device)_state.Sensors[n].Clone());

}

}

// Return the list

query.ResponsePort.Post(new brick.GetSensorsResponse(list));

yield break;

}

The code builds a new device list to send back. Notice that it clones the requested devices from the state.

Creating a New Drive Service

For the Integrator you should implement the generic Differential Drive service so that it can be controlled by existing applications such as the Dashboard and TeleOperation. However, you might want to keep everything together in a single DLL, rather than have separate DLLs for each service.

First create a new Drive service:

C:\Microsoft Robotics Studio (1.5)\ProMRDS\Chapter17>dssnewservice

/service:”IntegratorDrive” /namespace:”ProMRDS.Robotics.Integrator.Drive” /year:”2008” /month:”01” /alt:”http://schemas.microsoft.com/robotics/2006/05/drive.html” /i:”..\..\bin\RoboticsCommon.dll”

775

www.it-ebooks.info

Part IV: Robotics Hardware

This creates an empty service based on the differential drive contract in a separate folder. Now you can take the IntegratorDrive.cs and IntegratorDriveTypes.cs files and move them into the existing solution folder for the Integrator. Then, in Visual Studio, use Project Add Existing Item to add the files to the solution. This places the Drive service inside the same DLL as the Brick service.

Command Overload!

Implementing the various operations in IntegratorDrive.cs seems easy at first glance, especially as the brick implements SetDrivePower. However, taking the simple approach of sending all drive requests to the brick causes a subtle problem that only surfaces once you start to use a joystick. When you “wiggle” a joystick, it can generate dozens of position updates per second. As you have already seen, the Integrator has a slow CPU and commands are sent synchronously. As a result, commands to the motors back up in the internal communications port inside the Brick service. Therefore, long after you have stopped playing with the joystick, the robot is still faithfully executing your instructions. It might be fun to watch, but it makes the robot difficult to control.

In a TeleOperation environment, it is known that delays of more than half a second between action and response make it difficult for a human to control a robot. A delay of more than a second makes it almost impossible because most people are used to virtually instantaneous responses when they move their hands. A backlog of commands acts like a delay. Even worse, how can you stop a runaway robot that has hundreds of queued-up commands?

This “flooding” problem is a common flaw in drive services, and it results from the implicit assumption that requests are executed almost instantaneously. This might be true in simulation, but in real-world robotics, nothing is instantaneous.

Consider a serial port running at 9600 baud, 8 bits, no parity. It takes 10 “bit times” per character (start bit, 8 data bits, and a stop bit), so you can send roughly 960 characters per second. That’s over a millisecond per character, without allowing for time between characters or processing delays on the robot. In that time, the CCR might process 50 or 100 messages. It’s easy to see how a backlog can develop.

The answer to this dilemma is to always clear out the queue first, and only ever execute the most recent command. The robot might not execute exactly the sequence of commands that correspond to how you move the joystick around, but if you tell it to stop by releasing the joystick, it will stop immediately.

Your first thought might be to look at the length of the queue on the SetDrivePower port and clear it out. Unfortunately, the queue length is always zero. The SetDrivePower operation is one of many that are handled by an Interleave, and it is Exclusive. Therefore, incoming messages are held in the interleave’s pending queue and only passed to the handlers one at a time. This queue contains a mixture of different message types and you can’t easily extract all of the SetDrivePower requests.

The solution to this problem is to funnel all of the SetDrivePower requests from the handler through to another internal port. To ensure that only one request is executed at a time, you use a nonpersistent receiver on the internal port and set up a new receiver after each request has been processed. This port is declared at the top of IntegratorDrive.cs:

// Used for driving the motors. Always execute the newest pending drive request. private Port<drive.SetDrivePower> _internalDrivePowerPort =

new Port<drive.SetDrivePower>();

776

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

The SetDrivePower handler posts a message to the internal port, rather than send the command directly to the robot:

///<summary>

///SetDrivePower Handler

///</summary>

///<param name=”update”></param>

///<returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)]

public virtual IEnumerator<ITask> SetDrivePowerHandler(

drive.SetDrivePower update)

{

// Return a Fault if the drive is not enabled if (!_state.IsEnabled)

{

update.ResponsePort.Post(Fault.FromException( new InvalidOperationException(“Drive is not enabled”)));

yield break;

}

// All motion requests must do this first! ClearPendingStop();

if (update.Body.LeftWheelPower == 0 && update.Body.RightWheelPower

== 0)

_state.DriveState = drive.DriveState.Stopped;

else

_state.DriveState = drive.DriveState.DrivePower;

// Pass the request to the internal handler _internalDrivePowerPort.Post(update);

}

The internal handler, at the bottom of the source file, clears out any extra requests waiting in the port and then processes the most recent one:

///<summary>

///Process the most recent Drive Power command

///When complete, self activate for the next internal command

///</summary>

///<param name=”driveDistance”></param>

///<returns></returns>

public virtual IEnumerator<ITask> InternalDrivePowerHandler(drive.SetDrivePower power)

{

try

{

//Take a snapshot of the number of pending commands at the time

//we entered this routine.

//This will prevent a livelock which can occur if we try to

//process the queue until it is empty, but the inbound queue

(continued)

777

www.it-ebooks.info

Part IV: Robotics Hardware

(continued)

//is growing at the same rate as we are pulling off the queue. int pendingCommands = _internalDrivePowerPort.ItemCount;

//If newer commands have been issued, send success responses

//to the older commands and move to the latest one drive.SetDrivePower newerUpdate;

while (pendingCommands > 0)

{

if (_internalDrivePowerPort.Test(out newerUpdate))

{

//Timed motion requests do not have a response initially,

//that comes later when the motors are stopped

if (power.ResponsePort != null) power.ResponsePort.Post(

DefaultUpdateResponseType.Instance);

power = newerUpdate;

}

pendingCommands--;

}

yield return Arbiter.Choice(_brickPort.SetDrivePower( new drive.SetDrivePowerRequest(power.Body.LeftWheelPower, power.Body.RightWheelPower)),

delegate(DefaultUpdateResponseType ok)

{

_state.LeftWheel.MotorState.CurrentPower = power.Body.LeftWheelPower;

_state.RightWheel.MotorState.CurrentPower = power.Body.RightWheelPower;

if (power.ResponsePort != null) power.ResponsePort.Post(ok);

},

delegate(Fault fault)

{

if (power.ResponsePort != null) power.ResponsePort.Post(fault);

}

);

}

finally

{

// Wait one time for the next InternalDrivePower command Activate(Arbiter.ReceiveWithIterator(false,

_internalDrivePowerPort, InternalDrivePowerHandler));

}

yield break;

}

778

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

Note a couple of key points here:

The routine gets the current queue length and then removes only that number of requests. If it kept looping and removing requests until the queue length was zero, it is possible that it could loop forever.

When requests are taken out of the queue to be discarded, a response is sent back to the sender. This is essential because the sender might be waiting on completion of the request, and it would hang forever if the request were simply thrown away.

Notice that there is a check to see whether the ResponsePort is null. This is discussed in the next section. Requests with a null ResponsePort are the initial requests that are generated by timed motions (DriveDistance and RotateDegrees). It is more useful to notify the sender once the motion has completed, rather than when it starts, so these operations set the ResponsePort to null before posting a SetDrivePower request to the internal port. When the motion completes, a response message is sent when the stop request is processed. The mechanism for this is explained later in the section “Timed Operations.”

Having found a request to execute, the code sends it to the brick. The _brickPort is set up at the top of the code where the brick is declared as a partner service:

///<summary>

///Integrator Brick Partner

///</summary> [Partner(“Integrator”, Contract =

“http://www.promrds.com/contracts/2008/01/integrator.html”, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate, Optional = false)]

private brick.GenericBrickOperations _brickPort = new brick.GenericBrickOperations();

Lastly, the handler sets up a new nonpersistent receiver to get the next message from the internal port. The Start method kicks off the whole process by setting up the initial receiver.

There is another benefit of this approach: The timed operations (DriveDistance and RotateDegrees) now mesh nicely with the SetDrivePower operations. They are all Exclusive operations, but the timed operations cannot afford to wait until their operations complete or they would block all other requests. There has to be some way to cancel a timed operation that is in progress. Splitting the timed operations into two requests — start the motors running and stop the motors some time later — solves this problem.

Timed Operations

In the Dashboard (in Chapter 4), the arrow buttons in the user interface use the DriveDistance and RotateDegrees operations. Because these are very handy functions to have, it is a good idea to implement them even on robots that don’t have wheel encoders and therefore cannot make accurate moves.

To emulate these operations, the code calculates a time delay that corresponds as closely as possible to the amount of time required to complete the requested move. Then it starts the motors, waits for the delay period, and finally stops the motors. If these operations did not return until after the delay, then they could never be interrupted because they are Exclusive operations. This is not a good design, especially if the robot is about to crash and the Stop button has no effect!

779

www.it-ebooks.info

Part IV: Robotics Hardware

Therefore, these operations set a timer to fire off a Stop request after the appropriate time interval has elapsed. The only problem with this approach is, what if another command arrives during the delay period? You don’t want the Stop to be executed if a new drive command arrives before the delay has expired. Therefore, you have to cancel any pending Stop command (there will only ever be one of them) whenever another command arrives.

To make a long story short, the requirement to be able to interrupt the DriveDistance or RotateDegrees operations adds a lot of extra logic and complexity. However, as is the case with many parts of MRDS, once you have some working code, you can reuse it in similar services. The same code is therefore used for the Hemisson robot.

Consider the RotateDegrees handler. After checking whether the drive is enabled, the first thing it does is to cancel any pending Stop request. All the drive handlers do this, but it was not pointed out on the SetDrivePower handler earlier. Then it checks the rotation angle, because zero is pointless, and sets the wheel power based on the direction of rotation:

///<summary>

///RotateDegrees Handler

///</summary>

///<param name=”update”></param>

///<returns></returns> [ServiceHandler(ServiceHandlerBehavior.Exclusive)]

public virtual IEnumerator<ITask> RotateDegreesHandler(

drive.RotateDegrees update)

{

//This handler is basically the same as DriveDistance except for the

//calculation of the distance. It could use DriveDistance, but that

//is an unnecessary overhead.

//See DriveDistance for more comments.

//Time delay for the motion

int delay;

//Speeds for wheels to move at double leftSpeed;

double rightSpeed;

//Distance wheels will travel during rotation double arcDistance;

if (!_state.IsEnabled)

{

update.ResponsePort.Post(Fault.FromException( new InvalidOperationException(“Drive is not enabled”)));

yield break;

}

//All motion requests must do this first! ClearPendingStop();

//Check for zero degrees first because we don’t want the wheels

to jerk

// if the robot is supposed to go nowhere! if (update.Body.Degrees == 0.0)

780

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

{

update.ResponsePort.Post(new DefaultUpdateResponseType()); yield break;

}

else if (update.Body.Degrees < 0.0)

{

//Note that the speeds are opposite to make the robot

//rotate on the spot

leftSpeed = update.Body.Power; rightSpeed = -update.Body.Power;

}

else

{

leftSpeed = -update.Body.Power; rightSpeed = update.Body.Power;

}

Next, it calculates the time to move based on the distance the wheels will travel around the circumference of a circle with the diameter equal to the wheel base (the distance between the wheels). The property DistanceBetweenWheels is one of the standard properties in the generic Differential Drive state. It must be filled in during service initialization. Then it posts a SetDrivePower request to the internal port:

//Calculate the distance that the wheels must travel for the given

//turn angle. They will move around the circumference of a circle

//with a diameter equal to the wheel base. Each 360 degrees of

rotation

// is one full circumference.

arcDistance = Math.Abs(update.Body.Degrees) * _state.DistanceBetweenWheels * Math.PI / 360;

// Calculate the delay time

delay = CalculateTimeToMove(arcDistance, update.Body.Power);

//Start the motors running (in opposite directions) drive.SetDrivePower power = new drive.SetDrivePower();

//Set the power

power.Body.LeftWheelPower = leftSpeed; power.Body.RightWheelPower = rightSpeed;

//Do NOT respond to this message (Happens later on Stop) power.ResponsePort = null;

//Use the internal handler for throttling

_internalDrivePowerPort.Post(power); _state.DriveState = drive.DriveState.RotateDegrees;

// Set the timer and forget about it SetStopTimer(delay, update.ResponsePort);

}

The fun is not over yet. The timed operations are not supposed to send back a response until they have completed. Therefore, the handler cannot post back a reply straight away — the response must be posted when the Stop command is executed. The internal port handler has to be able to distinguish between normal SetDrivePower requests, which are acknowledged immediately, and DriveDistance and

781

www.it-ebooks.info

Part IV: Robotics Hardware

RotateDegrees, which are not acknowledged until the Stop request. Therefore, the message sent on the internal port has its ResponsePort set to null, and the original ResponsePort is passed to the

SetStopTimer routine:

// Set a timer to post a Stop later

void SetStopTimer(int delay, PortSet<DefaultUpdateResponseType, Fault> p)

{

int num;

//Classic case of “should never happen” ClearPendingStop();

//Increment the ACK counter to make it unique and

//keep a copy for later

_ackCounter++; num = _ackCounter;

//Remember the current response port in case it has

//to be cancelled

_ackPort = p; // Enable ACK

_ackPending = true;

//Setup a delegate for the specified time period

//It will post a StopMotion message when it fires Activate(Arbiter.Receive(

false,

TimeoutPort(delay), delegate(DateTime timeout)

{

// Send ourselves a Stop request using the ACK number _internalStopPort.PostUnknownType(new StopMotion(num));

}

));

}

SetStopTimer uses some global variables and has its own internal port to which Stop messages are posted. It keeps a counter that uniquely identifies the Stop request and sets a flag indicating that a stop is pending. This enables ClearPendingStop to cancel the request and not worry about the timer that has been set on the _internalStopPort. Remember that other requests might arrive while the timer is running, and they all call ClearPendingStop as their first step:

//Issue a fault if a pending Stop was terminated and

//clear the flag

void ClearPendingStop()

{

if (_ackPending)

{

_ackPending = false; Fault f;

f = Fault.FromException(new Exception(“Stop after timed motion aborted”));

_ackPort.Post(f); _ackPort = null;

}

}

782