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

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

if cmd = “H” then goto SetHigh

else

goto SetLow endif

endif

SetHigh: high param

goto MainLoop

SetLow: low param

goto MainLoop

To read the value of a pin, the ReadPin routine is called:

‘Read a single Pin

‘Requires pin number as a parameter

ReadPin:

gosub GetHex

‘ Only pins 0-7 are supported if param > 7 then

serout SERIAL_OUT,T9600_8,(“? Bad Pin”,CR,LF) else

b2 = pins

‘Get bit mask and extract pin status lookup param,(1,2,4,8,16,32,64,128),b3 b2 = b2 & b3

‘Send back result

if b2 <> 0 then

serout SERIAL_OUT,T9600_8,(cmd,”1”,CR,LF) else

serout SERIAL_OUT,T9600_8,(cmd,”0”,CR,LF) endif

endif

goto MainLoop

It is much quicker to read all of the input pins at once instead of one at a time, so there is a function for this too: ReadPins. It sends the information back as two hex digits. Although there is a function for it, setting all of the output pins is not feasible because one of them is Bluetooth Serial Out, and if you write to this pin, you will interfere with the serial communications.

If you want to know more about how the monitor program works, open Monitor.bas and read the code. It is fairly well commented.

Creating the Integrator Robot Services

The first step to creating the new Integrator robot service is to make a new service for the brick. Now that you have a Generic Brick service, and a defined communication protocol for the brick, this is fairly easy. To create a new service based on the Generic Brick contract, you use DssNewService with the /alt

763

www.it-ebooks.info

Part IV: Robotics Hardware

(alternate contract) and /i (implement) parameters to specify which service you want and the assembly to look in for the contract.

At the MRDS command prompt, enter the following command (shown in bold text) to create the Integrator service (all on one line):

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

/service:”Integrator” /namespace:”ProMRDS.Robotics.Integrator” /year:”2008” /month:”01” /alt:”http://www.promrds.com/contracts/2008/01/genericbrick.html” /i:”..\..\bin\GenericBrick.Y2008.M01.dll”

Don’t execute this command in your ProMRDS directory! The code already exists. If you want to try it out, make a temporary directory somewhere under the MRDS root and then run the command.

In the new Integrator solution, the service implementation file, Integrator.cs, contains stubs for all of the Generic Brick operation handlers. Don’t worry about them yet. Before you can implement the operations, you need to write code to communicate with the robot.

The new service has a reference to the Generic Brick DLL and a using statement already in the code:

using pxgenericbrick = ProMRDS.Robotics.GenericBrick.Proxy;

However, this is a bit of a mouthful, so the alias is shortened to brick. The new service does not have a reference to GenericBrickUtil.dll, containing the helper functions, so you need to add this. You also need to add a reference to RoboticsCommon.Proxy.

If you look in IntegratorTypes.cs, initially it contains nothing but the contract identifier. There is no need to define a main operations port or a set of message types — they all come from the generic brick.

Initialization

Because this first version of the Generic Brick contract cannot be reconfigured, all of the sensors and actuators should be redefined every time the service is run. Otherwise, it would be possible for a user to edit the config file and change device details, perhaps with surprising consequences.

To assist in initializing the Sensors and Actuators in the state, a couple of enums are defined in the IntegratorTypes.cs file: HwIds and Pins. Note that a hardware ID and a pin are not the same thing. (Fortunately, no port numbers are involved on the Integrator.) This is not essential, but it helps to keep all of these “magic” numbers together in one place so that you can see if there are duplications or missing values.

When you create the lists of devices during service initialization, you can do it using the Device constructor, as shown here:

_state.Sensors.Add(new brick.Device( “IR Bumper”, (int)HwIds.IrBumper, brick.DeviceTypes.DigitalIn, brick.DeviceFunctions.Bumper, brick.Location.FrontCenter,

764

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

);

Note that the initial state of some outputs might not be zero (or off). This must be taken into account in the initialization code.

Serial Communications

Most of the samples provided with MRDS communicate with the robot via a serial port (although it is usually a virtual serial port over Bluetooth). However, if you look at the code for each of the implementations, you will see a range of different approaches to implementing the communications.

Part of the problem is that every robot is subtly different. The Integrator is not very fast and it does “bit banging” instead of using a hardware universal asynchronous receiver/transmitter (UART), so it loses characters if you send them too quickly. Therefore, a small delay is necessary between the characters that make up a command. For the Integrator, this is set to 10 milliseconds because anything less results in lost characters. This is one of the reasons why most commands are a single character — it minimizes the overhead.

You also have to pause between commands to give the robot time to “digest” the command and execute it. The delay between commands has to be determined by trial and error for every new type of robot. You can do this by sending repeated commands and increasing the delay until the robot stops missing commands. You can usually tell if it is missing commands because there will be timeouts on receiving the responses, i.e., the robot never sees a command so it does not respond. For the Integrator, this intercommand delay has to be about 75 milliseconds, which is quite long. (This is one of the properties in the state in the Configuration.)

Note that polling (which is covered later) is always occurring. Therefore, commands are constantly being sent to the robot. You cannot have a command interrupted by a polling request — commands sent to the robot must be queued. In addition, a command and its corresponding response must be treated as an atomic operation. As a consequence of these problems, the communications code needs to operate synchronously, i.e., send a command and then wait for a response (or a timeout) before proceeding with the next command. In between commands you must ensure that there is a reasonable delay.

Although the SerialPort class in .NET (defined in System.IO.Ports) supports asynchronous operations, it is not designed to operate in a CCR environment. The code in IntegratorControl therefore uses blocking reads and writes, and throws in a few calls to Thread.Sleep for good measure. This is all very antisocial as far as the CCR is concerned, so the main Integrator service class is flagged with an ActivationSettings attribute:

[ActivationSettings(ShareDispatcher = false, ExecutionUnitsPerDispatcher = 3)]

765

www.it-ebooks.info

Part IV: Robotics Hardware

This causes DSS to create a separate dispatcher and allocate three threads. This way, the Integrator service will not interfere with other services running on the DSS node — the serial communications code can block a thread without causing problems.

To isolate the serial communications functions from the rest of the service, there is a separate module called IntegratorControl.cs. This is not a separate service, just a separate class in its own source file. The IntegratorControl class is responsible for opening and closing the serial port and sending low-level commands to the robot. The main service should not need to know the details of the protocol for talking to the robot, although it is difficult to keep it entirely separate. In particular, there are routines called SetPin, SetPower, and SetLEDs (their names are self-explanatory). Consider the code for SetPin:

///<summary>

///Set Pin

///</summary>

///<param name=”pin”>Pin on the Brick</param>

///<param name=”state”>True/False = On/Off or High/Low</param>

///<returns>Port for the response from the robot</returns> public Port<byte[]> SetPin(int pin, bool state)

{

byte[] on = { (byte)’H’, (byte)’0’ }; byte[] off = { (byte)’L’, (byte)’0’ };

Command cmd = new Command(); if (state)

{

on[1] = (byte)((byte)’0’ + pin); cmd.CommandString = on;

}

else

{

off[1] = (byte)((byte)’0’ + pin); cmd.CommandString = off;

}

CommandPort.Post(cmd);

return cmd.ResponsePort;

}

This routine sends either a High (H) or Low (L) command and appends the pin number to the command. Notice that it sends the command by posting a message; it returns a response port. Routines inside IntegratorControl send serial commands to an internal port called CommandPort. The format of these messages is as follows:

///<summary>

///Serial Command

///</summary>

///<remarks>Used internally to send packets to the serial port</remarks> public class Command

{

public byte[] CommandString; public Port<byte[]> ResponsePort;

766

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

public Command()

{

CommandString = null;

// Make sure that there is always a response port ResponsePort = new Port<byte[]>();

}

}

The CommandString is a byte array and it is sent verbatim. All characters in the response up to, but excluding, the following carriage return and linefeed are sent back to the ResponsePort as an array of bytes.

The handler for these Command messages is as follows:

///<summary>

///Serial Port Command Handler

///</summary>

///<param name=”cmd”></param>

///<returns></returns>

///NOTE: This handler executes serial commands which block the thread private IEnumerator<ITask> CommandHandler(Command cmd)

{

byte[] response = new byte[0];

if (cmd.CommandString == null || cmd.CommandString.Length == 0)

{

//Pathological case -- no command!

//Somebody sent us an empty packet, so send back an empty response cmd.ResponsePort.Post(response);

}

else

{

//Send the packet and wait for a response SendPacket(cmd.CommandString);

//Got a response?

if (_responseCount > 0)

{

byte[] resp = new byte[_responseCount]; for (int i = 0; i < _responseCount; i++)

resp[i] = _response[i]; cmd.ResponsePort.Post(resp);

}

else

cmd.ResponsePort.Post(response);

}

// Wait one time for a new Command Arbiter.Activate(_taskQueue,

Arbiter.ReceiveWithIterator<Command>(false, CommandPort, CommandHandler));

yield break;

}

767

www.it-ebooks.info

Part IV: Robotics Hardware

The handler sends the command to the robot using SendPacket, which does not return until it has received a response or the command has timed out. (The timeout is set in the state Configuration.) An empty response indicates that some sort of error occurred, which in most cases is a timeout. The protocol requires the robot to echo all commands back to the PC, so there should always be a response. The response is then posted to the original sender of the command. This enables senders to wait for the response (if they want), but in the usual CCR fashion — by waiting on a port, which frees up threads. Only the thread running the command handler will block and become unusable.

Before finishing, the command handler sets up another receiver to get the next command from the CommandPort. (The process is kicked off in the constructor for the IntegratorControl class.) By using a nonpersistent handler, it is guaranteed that only one instance of the command handler can ever execute at a time. In effect, this implements an Exclusive behavior.

Notice that a separate task queue (and dispatcher) is used for the command handler. Do you get a sense of paranoia about blocking CCR threads? This is not necessary given the ActivationSettings on the service, but it illustrates another approach.

Lastly, the SendPacket routine is as follows:

// Carriage Return

static char[] CR = { ‘\r’ };

private DateTime _lastPacketTime;

//SendPacket()

//Baud Rate is 9600 which is approximately 960 chrs/sec

//which is about a millisecond per character.

//NOTE: SendPacket() operates SYNCHRONOUSLY, although it does

//have timeouts to prevent it from hanging forever.

private bool SendPacket(byte[] buf)

{

TimeSpan timeDiff; int msDiff;

bool ok = true;

//Always check! There could be outstanding messages when the

//connection is closed.

if (!_serialPort.IsOpen) return false;

Notice that the code first checks whether the serial port is open. During shutdown, the port might be closed while commands are still waiting in the queue, and this avoids an exception.

Next it checks to see how much time has elapsed since the last command. The _lastPacketTime is set after every command has been processed, so you can figure out how long ago it was. If the new command is too soon after the previous one, then the code waits for the difference in time. This is a better approach than always waiting for a fixed amount of time after a command:

//Check that this packet is not too close to the last one.

//It is ESSENTIAL that there is a delay after sending

//a command because the Integrator cannot process the

//commands very quickly and will start to lose data!

768

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

timeDiff = (DateTime.Now - _lastPacketTime); msDiff = timeDiff.Milliseconds;

if (msDiff < Delay)

{

// Wait for the difference in time Thread.Sleep(Delay - msDiff);

}

Now the code writes out the serial command and then reads the response. The read and write timeout values are set when the port is opened, so if there is no response within the specified period (currently 250 milliseconds, which is a very long time), an exception occurs. In this case, the code sends a carriage return to the robot to try to illicit a response. This causes the robot to stop (if the command is received properly), which is probably the safest approach. It would be possible to resend the original command, but command retries are not implemented.

_responseCount = 0;

try

{

int i;

//Throw away any left-over data in the input buffer _serialPort.DiscardInBuffer();

//Output the packet one byte at a time to give the

//Integrator time to digest the characters.

//Otherwise it gets a data overrun and locks up! for (i = 0; i < buf.Length; i++)

{

_serialPort.Write(buf, i, 1); Thread.Sleep(10);

}

}

catch (Exception ex)

{

errorCounter++;

Console.WriteLine(“Comms Write Error on command “ + (char)buf[0] + “ (Error count “ + errorCounter + “)”);

Console.WriteLine(“Exception: “ + ex.Message); // Try to re-synch

_serialPort.Write(CR, 0, 1); ok = false;

}

finally

{

_lastPacketTime = DateTime.Now;

}

if (!ok)

return false;

ok = true; try

(continued)

769

www.it-ebooks.info

Part IV: Robotics Hardware

(continued)

{

int i, val;

i = 0;

while ((val = _serialPort.ReadByte()) != ‘\r’)

{

_response[i] = (byte)val; i++;

}

_responseCount = i;

// Throw away the rest of the response (only a LF) _serialPort.DiscardInBuffer();

}

catch (Exception ex)

{

errorCounter++;

Console.WriteLine(“Comms Read Error on command “ + (char)buf[0] + “ (Error count “ + errorCounter + “)”);

Console.WriteLine(“Exception: “ + ex.Message); // Send a CR to try to re-synch _serialPort.Write(CR, 0, 1);

ok = false;

}

finally

{

_lastPacketTime = DateTime.Now;

}

if (!ok)

return false;

messagesSentCounter++; return true;

}

Note the use of DiscardInBuffer. This prevents unsolicited characters, noise, or leftover characters from a previous command from interfering with the current command.

The last point is that communications statistics are updated as well.

Handling Sensors Using Polling

Sensor information has to somehow get transferred from the robot to the PC. There are only two options:

The PC polls the robot for sensor data.

The robot periodically sends sensor data to the PC.

Polling is the preferred option in this case because the robot cannot simultaneously send and receive. In other words, once the monitor program on the PICAXE requests a character from the serial port, it is forced to wait (block) until a character arrives. This is a deficiency in the firmware, although it actually arises from the use of “bit banging.”

770

www.it-ebooks.info

Chapter 17: Writing New Hardware Services

The main Brick service must therefore send commands to the robot at regular intervals. This is easy to set up using a timer. Toward the bottom of Integrator.cs is a region called Polling. To kick off the polling process, SetPollTimer is called from the Start method once a connection to the robot has been successfully established:

///<summary>

///Set a Timer for Polling

///</summary>

//NOTE: This does NOT guarantee an exact polling interval.

//Instead it adds the polling interval to the current time.

//There is only ever one receiver active at a time. private void SetPollTimer()

{

//Stop the timer if we are shutting down

if (_shutdown) return;

int wait = -1;

// If the PollingInterval is negative, then polling is off if (_state.Configuration.PollingInterval >= 0 &&

_state.Configuration.PollingInterval < _minimumPollingInterval) wait = _minimumPollingInterval;

else

wait = _state.Configuration.PollingInterval;

if (wait >= 0)

Activate(Arbiter.Receive(false, TimeoutPort(wait), PollingTimerHandler));

}

The PollingInterval is one of the few configuration parameters that can be changed. If it is negative, no polling occurs. If it is zero or less than a minimum value, it is set to the minimum. For Bluetooth with a slow serial port (9600 baud in this case), a minimum of 50 milliseconds is optimistic. In fact, you cannot poll the Integrator faster than about 10 Hz, which is a polling interval of 100ms.

The handler for the polling timer is as follows:

///<summary>

///Polling Timer Handler

///</summary>

///<param name=”time”></param>

private void PollingTimerHandler(DateTime time)

{

//Stop the timer if we are shutting down if (_shutdown)

return;

//Ignore timer if not connected, but keep it ticking over if (!_state.Connected)

{

SetPollTimer();

return;

}

(continued)

771

www.it-ebooks.info

Part IV: Robotics Hardware

(continued)

// Is Polling turned off?

if (_state.Configuration.PollingInterval < 0)

{

// Set the timer, but don’t do anything SetPollTimer();

return;

}

// Get Sensors Port<byte[]> response; response = _control.Poll();

Activate(Arbiter.Choice(

Arbiter.Receive<byte[]>(false, response, ProcessPollResult),

// Make sure that we don’t hang here forever!

Arbiter.Receive<DateTime>(false, TimeoutPort(1000),

SetPollTimer)));

}

The top of the routine contains a couple of safety checks. Then it calls the Poll method in the

IntegratorControl class.

Notice that the code waits on a response from the poll request, or a timeout of one second. This shows how to avoid getting hung if a response never arrives. In this case, it is unnecessary because the serial port code has a 250ms timeout for the write and read, but the principle is important. As an aside, there is an overloaded version of SetPollTimer that accepts a DateTime that can be used for the timeout.

The last step is to process the results of the poll:

///<summary>

///Handle results from a Poll and reset Timer

///</summary>

///<param name=”result”></param>

private void ProcessPollResult(byte[] result)

{

//NOTE: The result should always be 3 bytes long or there is

//something wrong

if (result.Length == 3)

{

bool changed = false;

//Process the response

//Format should be Ixx

if (result[0] == (byte)’I’)

{

// Convert the hex result to binary int n = HexByteToInt(result, 1);

for (int i=0; i<_state.Sensors.Count; i++)

{

// Only process Digital sensors this way

if (util.IsDigitalDevice(_state.Sensors[i]))

772