Задани на лабораторные работы. ПРК / Professional Microsoft Robotics Developer Studio
.pdf
www.it-ebooks.info
Chapter 16: Autonomous Robots
Figure 16-9
The keyboard handlers are in a region at the bottom of the Form code:
#region Key Handlers
//NOTE: The arrow keys will not normally appear in a KeyDown event.
//This is because they will be pre-processed. However, the operation
//under CF seems to be different and it does no harm to leave them
//in here anyway.
private void StingerDriveByWireForm_KeyDown(object sender, KeyEventArgs e)
{
switch ((Keys) e.KeyValue)
{
case Keys.Up: case Keys.W:
Forward(); e.Handled = true; break;
... (code omitted for brevity) ...
case Keys.Escape: btnExit_Click(sender, e); e.Handled = true;
break;
default:
break;
}
}
// Stop the motors any time that a key is released
private void StingerDriveByWireForm_KeyUp(object sender, KeyEventArgs e)
(continued)
693
www.it-ebooks.info
Part IV: Robotics Hardware
(continued)
{
Stop();
}
// Overriding the ProcessDialogKey handler allows us to trap the arrow keys. protected override bool ProcessDialogKey(Keys keyData)
{
switch (keyData)
{
case Keys.Up: Forward(); return true;
... (code omitted for brevity) ...
case Keys.Right: TurnRight(); return true;
default:
break;
}
return base.ProcessDialogKey(keyData);
}
#endregion
First, notice that the KeyDown event is used to send a motion command to the robot. (For the convenience of gamers, the A, S, D, and W keys can be used instead of the arrow keys, but obviously this only applies to the desktop version of the service). This event occurs as soon as a key is pressed.
Conversely, a KeyUp event occurs when a key is released. The handler for KeyUp immediately sends a Stop command without even looking to see which key was released. The combined effect of these two handlers is that the robot will continue to move as long as you hold down a key. Note that if you “tap” a key, the robot might jerk a little, but it will not keep driving.
For the PDA version, these two event handlers are all that is required, provided that you change the KeyPreview property for the Form to true. The arrow keycodes (Up, Down, Left, and Right) are generated by the “rocker switch” on the PDA and passed to the keyboard event handlers.
The Windows XP version is a little more complicated. In addition to setting the Form’s KeyPreview property to true, you also need to override the ProcessDialogKey handler for the Form. This handler must return true if it handles a particular keystroke. If it does not want to handle the key, it can simply pass the keystroke back to the regular handler. See Chapter 4 for more details.
Service Operations
The messages that the Form can send are declared in StingerDriveByWireTypes.cs and included
in the PortSet for the main service operations. Obviously, the Form has to send commands to the main service whenever one of the buttons is pressed:
[ServicePort]
public class StingerDriveByWireOperations : PortSet
694
www.it-ebooks.info
Chapter 16: Autonomous Robots
{
//These changes from the previous way of declaring a PortSet
//are necessary to work with CF when you have more than 8
//ports in the set
public StingerDriveByWireOperations() : base(
typeof(DsspDefaultLookup),
typeof(DsspDefaultDrop),
typeof(Get),
typeof(Replace),
typeof(Load),
typeof(MotionCommand),
typeof(Wander),
typeof(Quit)
)
{}
This code illustrates another difference between the desktop version of MRDS and the CF version. Under the CF version, only a maximum of eight ports can be declared in a PortSet using the usual syntax:
public class xxxOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop, Get>
{
}
The alternative form of declaration using typeof gets around that problem, but at a cost — the strong compile-time type-checking that used to exist on the Post method is lost and the types are not checked until runtime. Even with this alternative method, there is still a limit of 20 operations in total.
You can compensate for this loss of type-checking by overloading the Post method as follows, but you have to do this for each of the operations individually:
public void Post(MotionCommand m)
{
PostUnknownType(m);
}
The MotionCommand operation warrants a little more explanation. This particular operation handles all of the possible motions: Forward, Backward, Turn Left, Turn Right, and Stop. It is much better to condense several operations into one message type than to have several separate operations. Because there is a limit of 20 operations in the CF environment, combining them wherever possible makes sense.
The code for the MotionCommand class in StingerDriveByWireTypes.cs includes some additional constructors:
public class MotionCommand : Update<MotionRequest,
PortSet<DefaultUpdateResponseType, Fault>>
{
public MotionCommand()
: base(new MotionRequest())
{
}
(continued)
695
www.it-ebooks.info
Part IV: Robotics Hardware
(continued)
public MotionCommand(MotionRequest body) : base()
{
this.Body = body;
}
public MotionCommand(int left, int right) : base()
{
this.Body.LeftPower = left; this.Body.RightPower = right;
}
}
[DataContract]
[DataMemberConstructor] public class MotionRequest
{
private int _leftPower; private int _rightPower;
[DataMember, DataMemberConstructor(Order=1)] [Description(“Left Motor Power”)]
public int LeftPower
{
get { return this._leftPower; } set { this._leftPower = value; }
}
[DataMember, DataMemberConstructor(Order=2)] [Description(“Right Motor Power”)]
public int RightPower
{
get { return this._rightPower; } set { this._rightPower = value; }
}
public MotionRequest()
{
}
public MotionRequest(int left, int right)
{
this._leftPower = left; this._rightPower = right;
}
}
Notice the use of the attribute [DataMemberConstructor] under [DataContract] and on class members, which causes DssProxy to generate helpers (also called constructors in the text) in the Proxy DLL. These helper methods are the same as the ones you see declared in MotionRequest. Only two constructors can be created this way — one with no parameters and another with the parameters that are specified using the [DataMemberConstructor] attributes (or all of the public fields if no attributes are used on individual data members).
696
www.it-ebooks.info
Chapter 16: Autonomous Robots
The Order parameter enables you to specify the order in which the fields will appear in the constructor, or not at all if Order = -1.
The reason for declaring the constructors in the class declarations is so that they can be used in the Windows Form code. This service cannot use its own Proxy, so the automatically generated constructors are not available. Unfortunately, this is a “chicken or the egg” situation because DssProxy generates the constructors when you compile, but you need them defined so that the service will compile …
The last step in this puzzle is back in StingerDriveByWireOperations where a shorthand method is declared for sending MotionCommand messages:
public virtual PortSet<Microsoft.Dss.ServiceModel.Dssp.DefaultUpdateResponseType,Fault> MotionCommand(int leftPower, int rightPower)
{
MotionRequest body = new MotionRequest( leftPower, rightPower); MotionCommand op = new MotionCommand(body);
this.Post(op);
return op.ResponsePort;
}
With all of these declarations in place, the Windows Form can make motion requests simply by using the shorthand version, such as in the Forward method, which drives the robot forwards. This method is called by the event handler for the up arrow button on the Form or the keyboard handler when the up arrow key or W is pressed:
private void Forward()
{
//This sample sets the power to 75%.
//Depending on your robot hardware,
//you may wish to change these values.
//Use the short-hand message sender as an example. _mainPort.MotionCommand(75, 75);
}
This slightly long-winded explanation shows how it is possible to create operations in the Proxy DLL that other services can use. It considerably simplifies sending messages.
Lastly, note that the buttons on the Form are “sticky,” meaning that when you click a button, the robot will continue to move in that direction — you don’t have to hold down a button as you do with a key. In order to stop the robot, you must click the Stop button.
Interfacing to the Serializer Board
The Serializer services do not implement a generic Differential Drive so you have to partner directly with the motors. (The manifest specifies Motor1 and Motor2 partners). You do not need to partner with the Serializer itself, although the service does this so that it can send a Shutdown message to the Serializer:
An additional service, Stinger PWM Drive, is included with the code for this chapter that implements the generic Differential Drive contract. It is not discussed here.
697
www.it-ebooks.info
Part IV: Robotics Hardware
//Notice that we only partner with one motor because you can send
//a request to a motor service with either ID and it will work [Partner(“Motor1”,
Contract = PwmMotorProxy.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
PwmMotorProxy.MotorOperations _motorPort = new PwmMotorProxy.MotorOperations(); PwmMotorProxy.MotorOperations _motorNotifyPort = new
PwmMotorProxy.MotorOperations();
//For unsubscribing
Port<Shutdown> _motorShutdownPort = new Port<Shutdown>();
The Stinger Drive-By-Wire service subscribes to the PWM (Pulse Width Modulation) Motor Service, but this is not strictly necessary to use the motors. The notification messages are only used to display the current motor power.
When a motion request arrives from the Form, it is processed by the MotionHandler:
[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public virtual IEnumerator<ITask> MotionHandler(MotionCommand motion)
{
SetMotors(motion.Body.LeftPower, motion.Body.RightPower); yield break;
}
This calls the SetMotors method, which sends SetMotorPower messages to each of the motors.
Note that if the Stinger supported the generic Differential Drive contact, then the preceding code would send a SetDrivePower request to the Stinger drive instead of calling SetMotors.
//Common routine to handle all motor commands
//NOTE: This should not be called directly. Post a MotionCommand
//to the main port so that the requests will be queued (it is an
//Exclusive handler). There are a couple of places where this rule
//is violated because it is safe to do so.
void SetMotors(int motor1Power, int motor2Power)
{
if (_state.MotorEnabled)
{
//Add a coordination header to the motor requests
//so that advanced motor implementations can
//coordinate the individual motor requests.
//Note that there are TWO items in the coordination. coord.ActuatorCoordination coordination = new
coord.ActuatorCoordination(); coordination.Count = 2;
//Set up the requests PwmMotorProxy.SetMotorPowerRequest req1 = new
PwmMotorProxy.SetMotorPowerRequest(); req1.Id = 1;
req1.TargetPower = motor1Power; PwmMotorProxy.SetMotorPower leftSmp = new
PwmMotorProxy.SetMotorPower(req1);
698
www.it-ebooks.info
Chapter 16: Autonomous Robots
leftSmp.AddHeader(coordination); PwmMotorProxy.SetMotorPowerRequest req2 = new
PwmMotorProxy.SetMotorPowerRequest(); req2.Id = 2;
req2.TargetPower = motor2Power; PwmMotorProxy.SetMotorPower rightSmp = new
PwmMotorProxy.SetMotorPower(req2);
rightSmp.AddHeader(coordination);
//Now post to both motors as quickly as possible just in case
//the coordination does not work
_motorPort.Post(leftSmp); _motorPort.Post(rightSmp);
}
else
{
//The motors are disabled (for debugging) but show the user
//what would have happened
Console.WriteLine(“Dummy Motors: {0},{1}”, motor1Power, motor2Power);
}
}
The MotionHandler uses an ActuatorCoordination to try to synchronize the changes to the motor power settings. Advanced services can use Coordinations to specify that a set of messages must all be received before taking an action. In this case there are two motors, so messages must be received in pairs. (The PWM Motor service does not seem to properly support Coordinations).
SetMotors uses the MotorEnabled state variable to control whether the motor power is actually changed or not. You can edit the config file to change this flag to false so that the motors on the Stinger will not be activated. This is handy during testing because the Stinger won’t run away from you; and it also conserves battery life. However, a message is still displayed on the console so that you know SetMotors was called.
Reading the Infrared Sensors
Three GP2D12 services run independently — one for each IR sensor. (The manifest specifies three partners: IRFront, IRLeft, and IRRight.) This makes partnering with them a little more complicated, but it illustrates an interesting way to obtain information about partners and establish the connections at runtime.
Each of the IR sensors has its own config file specified in the manifest. This enables each sensor to be associated with a different analog input pin. The Units settings in these config files must be metric (not English) because the values used by the Wander behavior are all in centimeters.
1. The necessary ports are declared at the top of the main service:
//Ports for the InfraRed sensors
//Notice that there is NO Partner attribute on this port
//See below for subscription Gp2d12Proxy.Gp2d12Operations _gp2d12Port; Gp2d12Proxy.Gp2d12Operations _gp2d12NotifyPort;
//For unsubscribing
(continued)
699
www.it-ebooks.info
Part IV: Robotics Hardware
(continued)
Port<Shutdown> _gp2d12ShutdownPort = new Port<Shutdown>();
// Port to receive Directory service notifications over for subscribing...
ds.DirectoryPort _directoryPort;
ds.DirectoryPort _directoryNotifyPort = new ds.DirectoryPort();
There is no partner declaration on the Gp2d12Operations port. Additional ports are declared for communicating with the DSS Directory service.
2. The Start method calls SubscribeToIRSensors, which in turn sends a subscription request to the Directory service and sets up a handler to receive notifications:
//Subscribe to the IR sensors
//NOTE: This is a round-about process because there are three
//IR sensors and we want to handle them all together
void SubscribeToIRSensors()
{
// Create a Directory Service port to receive notifications _directoryPort = 
ServiceForwarder<ds.DirectoryPort>(ServicePaths.InstanceDirectory);
//Send a subscription to the Directory service for the type of
//contract that we are interested in SendSubscriptionToDirectory(Gp2d12Proxy.Contract.Identifier);
Console.WriteLine(“Listening for subscriptions from Directory Service”); // Listen for Directory insertions
Activate(
Arbiter.Receive<ds.Insert>(true, _directoryNotifyPort, InsertNotificationHandler));
}
3. Subscribing to the Directory is a simple process:
// Send a subscription to the Directory Service for a type of Contract private void SendSubscriptionToDirectory(string identifier)
{
ds.SubscribeRequestType subBody = new ds.SubscribeRequestType(null, new ServiceInfoType(identifier));
//Create a subscribe message, and fill the body in...
ds.Subscribe sub = new ds.Subscribe(subBody); sub.NotificationPort = _directoryNotifyPort;
//Send the message to the Directory
_directoryPort.Post(sub);
// Set up a one-time receiver Activate(
Arbiter.Choice(
Arbiter.Receive<SubscribeResponseType>(false, sub.ResponsePort, delegate(SubscribeResponseType response)
{
}),
700
www.it-ebooks.info
Chapter 16: Autonomous Robots
Arbiter.Receive<Fault>(false, sub.ResponsePort, delegate(Fault f)
{
LogError(“Subscribe to directory service failed for “ + 
identifier);
})
)
);
}
When a new notification arrives from the Directory service, it is processed to determine whether the contract identifier is one of the desired partners; if it is, a subscription is made to the service to receive updates:
// Process Directory insertions
void InsertNotificationHandler(ds.Insert insert)
{
//See if the new service in the Directory is one that we are
//interested in
if (insert.Body.Record.Contract == Gp2d12Proxy.Contract.Identifier)
{
// Create a new notification port for IR messages _gp2d12NotifyPort = new Gp2d12Proxy.Gp2d12Operations();
// Get a service forwarder so we can send messages to the IR service _gp2d12Port = 
ServiceForwarder<Gp2d12Proxy.Gp2d12Operations>(insert.Body.Record.Service); Gp2d12Proxy.Subscribe subscribe = new Gp2d12Proxy.Subscribe(
new SubscribeRequestType());
subscribe.NotificationPort = _gp2d12NotifyPort;
//Set the Shutdown port too so we can unsubscribe later subscribe.NotificationShutdownPort = _gp2d12ShutdownPort;
//Now subscribe to the IR service
_gp2d12Port.PostUnknownType(subscribe);
// Set up a persistent receiver MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(), new ExclusiveReceiverGroup(
Arbiter.Receive<Gp2d12Proxy.UpdateReading> (true, _gp2d12NotifyPort, ReplaceIRHandler)
),
new ConcurrentReceiverGroup()
)
);
Console.WriteLine(“Subscribed to Gp2d12 Service”);
}
Note two other points about this code:
It adds a shutdown port to the subscription message. By sending a Shutdown message to this port later, it is possible to unsubscribe from the service.
701
www.it-ebooks.info
Part IV: Robotics Hardware
The handler uses CombineWith to add an Exclusive receiver to the existing set of receivers. If the handler had simply created a new receiver, then it would run independently and there could be multiple simultaneous instances of the receiver activated. The receiver should be part of the Exclusive group because it updates the state.
4. The last part of the code is the actual IR handler:
//Handle IR Sensor updates
//This handler is essential to the operation of the Wander behavior.
//However, it does not execute the behavior directly. Instead, the
//Wander behavior runs on a timer. This gives a far greater chance
//that all three of the IR sensors will have updated their values.
//Otherwise, we would be reevaluating the status after every new
//individual value, and this is not a good idea.
public void ReplaceIRHandler(Gp2d12Proxy.UpdateReading ur)
{
// Save the value switch (ur.Body.Pin)
{
case SerializerIoPin.Pin0: _state.IRLeft = ur.Body.Distance; break;
case SerializerIoPin.Pin1: _state.IRFront = ur.Body.Distance; break;
case SerializerIoPin.Pin2: _state.IRRight = ur.Body.Distance; break;
default:
break;
}
// Display the value if we have a GUI if (!_state.Headless)
{
_WinFormsPort.FormInvoke( delegate()
{
_driveForm.SetIRReadings((int)ur.Body.Pin, ur.Body.Distance);
}
);
}
...
This handler simply updates the state with the latest IR range value and then, if not operating headless, also updates the Form to display the current value. Although this handler is a key component of the Wander behavior, it does not directly call the Wander method. Instead, the Wander method runs on an independent timer, as explained in the next section.
702