Задани на лабораторные работы. ПРК / Professional Microsoft Robotics Developer Studio
.pdf
www.it-ebooks.info
Chapter 12: Visual Programming Examples
UpdateBumpsCliffsAndWalls notification is sent. If the SumoMode is not Combat, then the notification is ignored. If either or both of the bumpers are pressed, then the sumobot pushes ahead with all its might with a wheel speed of 500 set on both wheels. Finally, if neither of the bumpers is pressed and the current Manouver is “Ramming,” the robot calms down and goes back to its normal straight Wander mode.
The Manouver Message
The only part of the diagram we haven’t examined is what exactly happens when a Manouver message is sent to the PlayerOne activity. This action is shown in Figure 12-14.
Figure 12-14
As you can see, the implementation of this action is very simple. Three values are passed in: the name of the maneuver, the left wheel speed value, and the right wheel speed value. The name is stored in the Manouver variable for later reference and the left and right values are applied to the robot drive.
533
www.it-ebooks.info
Part III: Visual Programming Language
This diagram gives our little VPLSumo robot the basic behaviors of a sumobot but it is usually at a disadvantage to the sample sumo player shipped by Microsoft because it does not use its webcam to locate its opponent. You’ll examine how to do vision processing later in this chapter.
Arm Mover
In Chapter 8, you developed a simulated robotic arm based on the Lynxmotion L6 arm, and in Chapter 15 you’ll see how to write services for a real L6 arm. In this section, you’ll learn how to write a VPL diagram that can control the movements of the arm.
The project is appropriately titled MoveArm and you can find it, as you are certainly aware by now, in the ProMRDS\chapter12 directory. The idea is to use an Xbox controller to control a piece of hardware (or simulated hardware) to make the arm interactive or to be able to teach it movements.
You need an actual Xbox 360 controller connected to a USB port on your computer to make this diagram work. This controller, shown in Figure 12-15, has a number of controls on it that make it useful for controlling robots.
Left Thumbstick
Right Thumbstick
Figure 12-15
Both of the thumbstick controls provide X and Y offsets. In this application, you’ll use the left thumbstick horizontal direction to control the movement of the arm in the X direction. Pulling the left thumbstick to the left decreases the X position of the gripper, while pulling it to the right increases the X position. The vertical direction of the left thumbstick controls the Z position of the arm in the same way. You can think of the 2D thumbstick control as moving the arm in the 2D X-Z plane. The vertical direction of the right thumbstick controls the vertical or Y position of the arm. Finally, the angle of the wrist is controlled by the yellow Y button and the green A button.
534
www.it-ebooks.info
Chapter 12: Visual Programming Examples
It is recommended that you use a wired Xbox 360 controller rather than a wireless one because it is easier to configure it and get it working properly on a PC.
The main diagram for this project is shown in Figure 12-16.
Figure 12-16
The ArmMover Activity
Like the preceding project, it’s not much to look at. It consists of a single custom activity called ArmMover, which is initialized with an integer data value of 100. Obviously, all of the logic is in the ArmMover activity.
The Start action of the ArmMover activity is shown in Figure 12-17.
535
www.it-ebooks.info
Part III: Visual Programming Language
Figure 12-17
In this action, each of the variables that keeps track of the position of the arm is set to a default value. The diagram keeps track of the position of the arm gripper with the X, Y, and Z variables, which default to the position (-0.2, 0.15, 0). The Angle variable keeps track of the angle of the wrist and is initialized to 0. The SinA and CosA variables are used to store the sine and cosine of the wrist angle to make the conversion of the wrist pose to a quaternion easier; and they are initialized to the sine and cosine of the default wrist angle.
The SetPollInterval Action
The diagram for the SetPollInterval action is much larger because it contains all of the logic to move the arm so it will be shown in sections. Figure 12-18 contains the polling part of the action.
536
www.it-ebooks.info
Chapter 12: Visual Programming Examples
Figure 12-18
When an integer PollInterval value is set on the input, the PollInterval variable is initialized with it and a message is passed to the result pin to indicate that the SetPollInterval action message was successfully processed. The action never really completes, however, because it runs in a continuous loop, polling the Xbox controller.
The PollInterval value is passed to the Timer activity, which waits that number of milliseconds before sending a message to its output, which does two things:
It sends a message back around to the timer input so that the timer is fired on a regular cycle determined by the value of PollInterval.
It sends a Get message to the XInputController activity, which retrieves the current state of the controller. The action taken based on the movement of the left thumbstick is shown in Figure 12-19.
537
www.it-ebooks.info
Part III: Visual Programming Language
Figure 12-19
The thumbstick sends values between -1.0 and 1.0 in both the X and Y directions. The topmost If activity takes an action based on the horizontal position of the left thumbstick. If the thumbstick is pulled all the way to the right (> 0.8), then the X position of the arm gripper is increased by five millimeters. If it is pulled only partially to the right (> 0.1), then the X position is increased by only two millimeters. This enables the arm to be moved quickly but it also provides a finer degree of control for exact positioning. Similar actions are taken for negative values of the thumbstick.
The second If activity does the same thing but it uses the Y position of the thumbstick to set the Z position of the arm.
Figure 12-20 shows how the right thumbstick and buttons are handled.
538
www.it-ebooks.info
Chapter 12: Visual Programming Examples
Figure 12-20
The Y position of the right thumbstick is used to change the Y position of the arm, much like the Y position of the left thumbstick is used to change the Z position.
The Y and A buttons are handled in the bottom If activity. They decrease and increase the wrist angle by 1/100 of a radian, respectively. After the Angle variable has been updated with the new angle, the MathFunctions activity calculates the sine and cosine of the new angle and these are stored in the appropriate variables for later use.
Moving the Arm
The final section of the diagram to discuss is the part that actually moves the arm based on the values in the variables. This is shown in Figure 12-21.
539
www.it-ebooks.info
Part III: Visual Programming Language
Figure 12-21
This part of the diagram consists of only seven activity blocks. Their purpose is to continually present the position of the arm grip to the LynxL6Arm service using the SetEndEffectorPose message so that as the position variables are updated, the arm moves.
The PollInterval value is extracted from the input message and this begins the execution of the loop. This message propagates to the Merge activity, which then sends a SetEndEffectorPose message
to the LynxL6Arm activity with the inputs shown in the Properties Toolbox in Figure 12-22.
540
www.it-ebooks.info
Chapter 12: Visual Programming Examples
Figure 12-22
The X, Y, and Z variables feed the X, Y, and Z inputs of the EndEffectorPose message as you would expect. However, the angle of the wrist is specified in this message using a quaternion.
Don’t worry; there is no need to wade through pages and pages of explanations about quaternions. Suffice it to say that the angle of the wrist can be converted to a quaternion by specifying the sine of the angle in the X position and the cosine of the angle in the W position. These values were calculated and stored in variables when the wrist angle was updated.
When the LynxL6Arm activity has completed moving the arm to the specified position, it sends a success message, which feeds back to a Data block that is used to place an integer value on the wire so that it matches the other inputs to the Merge activity. This message is eventually fed back into the LynxL6Arm activity to position the arm again.
If the arm is unable to move to the position specified due to mechanical constraints, the LynxL6Arm activity sends a Fault message instead. To indicate to the user that the arm is in an invalid position, the SoundPlayer activity is used to sound a system beep and the Timer0 activity is used to wait for one second to give the beep time to complete before a new EndEffectorPose is presented to the LynxL6Arm activity.
When you run the diagram, you might hear a couple of beeps because the diagram sends messages to the Arm service before the Arm entity has been initialized in the Simulation Environment. As soon as the simulator window appears, the arm should reset to its default position and then you should be able to move it using the controller. When you attempt to move the arm to an invalid position, you will hear periodic beeps until you manipulate the controls to bring the arm back to a valid position.
541
www.it-ebooks.info
Part III: Visual Programming Language
The SetPollInterval action of the ArmMover activity has two loops that execute simultaneously. An action may have any number of internal loops or instances of recursion running at the same time.
This activity controls a simulated arm but you’ve done nothing that is specific to the simulated arm so it is possible to run it with an actual hardware arm. If you have a Lynxmotion L6 arm, you can run the diagram in the MoveArmHW folder to control it in the same way.
Two parameters can be set on the arm that the diagram does not handle: the wrist rotation and the grip open/close position. While this functionality is left as an exercise for you (don’t you just hate that?), here are some hints about how you might go about it.
You can set up another variable to store the wrist rotation with its own sine and cosine values, like the Angle variable. The quaternion passed to the SetEndEffectorPose input of the LynxL6Arm activity needs to include this angle information. The following code, which converts Euler angles to a quaternion, can be used for guidance:
double c1 = Math.Cos(heading / 2); double c2 = Math.Cos(pitch / 2); double c3 = Math.Cos(roll / 2); double s1 = Math.Sin(heading / 2); double s2 = Math.Sin(pitch / 2); double s3 = Math.Sin(roll / 2);
quat.W = (float)(c1 * c2 * c3 - s1 * s2 * s3); quat.X = (float)(s1 * s2 * c3 + c1 * c2 * s3); quat.Y = (float)(s1 * c2 * c3 + c1 * s2 * s3); quat.Z = (float)(c1 * s2 * c3 - s1 * c2 * s3);
The wrist angle is pitch and the wrist rotation angle is roll.
The gripper open and closed position can be set by using the SetJointTargetPose input of the LynxL6Arm and specifying the joint name Grip along with a joint position. Perhaps you could use the X and B keys on the controller to open and close the grip.
Line Follower
One of the classic basic robotics projects is to build a line-following robot. Typically, the robot has two color or brightness sensors mounted on the left and right side of the robot, facing the ground. The signals from the sensors control the motor power so that the robot follows a line on the floor. When the robot strays from the line, the left or right side motor is driven faster to correct the course.
The MRDS 1.5 release includes a line-follower diagram for the iRobot Create but it is not very handy if you don’t happen to have an iRobot Create, so this section presents a slightly different line-following algorithm that works with the Simulation Environment, along with a utility that can be used to generate line geometry on the floor of the Simulation Environment.
The LineMesh Utility
Every visible object in the Simulation Environment is represented by a 3D mesh of triangles. 2D images can be painted on these 3D meshes using texture maps, but texture maps tend to get very fuzzy when
542