# Path Planning

In this tutorial we are introducting the possibility of controlling a Parallax Boe-Bot robot using an overhead camera (possibly one mounted on the ceiling) to control the robot to move around an arena. It is assumed that the camera is stable , looking down onto the robot arena and does not move during the robot execution.

The following is what such a potential arena looks like from an overhead camera.

The small red marks are electrical tape stuck to a black metallic surface. The green arrow is a small piece of cardboard with green electrical tape attached in the form of an arrow. This cardboard is placed above a Parallax Boe-Bot in order to easily identify the robot from overhead.

The first step is to identify all the parts in the arena. This includes the waypoints and the robot plus its orientation. First we will start with detecting the Boe-Bot.

# Detecting the Robot

The above image shows the robot under the green triangle. To detect the triangle we use the RGBFilter module set to filter on green.

We can see that the triangle is there but it has some small holes in it. The Fill module with a parameter of zero will fill in any remaining holes.

To smooth out the triangle border and remove some of the additional noise we add the Erode module to produce a cleaner image. We also add in the Blob Filter and set it to eliminate all but the largest blob just in case some other green artifact remains after the erosion. This also provides us with a COG_X and COG_Y array with a single entry that contains the X, Y center of the triangle and is used to determine where the robot's position is.

That completes the green triangle (robot) extraction. Now we need to find out which direction the triangle is pointing so that we know which direction to turn the robot.

# Geometric Analysis

Once the green triangle has been segmented from the background we add the Geometric module to run an analysis against the triangle. The value in particular that we are looking for is the ANGLE parameter which gives an indication of orientation of the blob.

When viewing this parameter you will notice that it flickers +=10 degrees or more. This can be reduced by adding in other blob smoothing modules such as Smooth_Hull but this additional precision is not needed for our purposes. Note that the angle is sensitive to the outline or border pixels of the blob. If you make the green triangle ends sharper the stability will also increase.

Once this number has been calculated we save the generated numbers (ANGLE, COG_X, COG_Y) to other variables so they don't get overwritten. This is accomplished using the Statements->Set Variable module. Once this module executes the variables are now called robot_orientation, robot_x and robot_y.

Using the Other->Marker module we revert the image back to the source in preparation for extracting the waypoints.

# Waypoint Extraction

Using the Marker module the current image has now reverted back to the original source. We can once again apply the RGBFilter but this time for red.

To again remove the noise we add in an Erode and Blob Filter to reduce the number of blobs to the 4 largest.

The Blob filter module again leaves us with an array of BLOBS with 4 sets of x, y coordinates. To keep these points from getting overwritten by other modules we use the Set Variable module to make a copy of them as "waypoints".

Now it is time to put all the parts together.

# Path Planning

The Path Planning module will use the waypoints and the current robot position to plan a path through all the waypoints.

Note that the configuration for the Robot location uses the variables we saved back when detecting the robot position. Also note the use of the variable "waypoints" to automatically provide the waypoints based on a variable as apposed to the manual interface seen directly above.

The graphics created by the path planning module is then overlaid on the current image to view the final planned path.

There remains one problem surrounding the waypoint generation. If we generate the waypoints on each new image frame the robot will obscure the waypoints as it nears them. This will cause a chaotic condition as the system will suddenly lose the waypoint and direct the robot to another. Once the robot moves to uncover the last waypoint it will again be directed to that waypoint causing an oscillation.

To remove this issue we only generate the waypoints once and use the reset waypoints trigger in the Path Planning module to regenerate the waypoint list. This resolution can be seen by the usage of the IF statement in the middle of the pipeline.

Now that we have the planned route we need to determine how and when to move the robot.

# Robot Control

The Path Planning module generated a PLAN_ORIENTATION variable which tells us what direction the next waypoint is from the current robot position. We need to approximately orient the robot to match that direction.

Since we know the current robot orientation from the ANGLE variable which was renamed to robot_orientation we can compare that variable with the PLAN_ORIENTATION variable to determine how to turn the robot. Once the robot is in that general direction we can just move the robot forward and repeat the checking process. This control logic is performed using a script entered in the VBScript module.

' define some constants to use for speed. Note that the boe-bot
' moves forward with left and right being opposite numbers (due
' to the flipping of the servos as part of the robot
' construction)
left_speed 115
right_speed 105
rev_left_speed 105
rev_right_speed 115
stopped 110

' this is the orientation produced by the path_planning and the
' orientation we want the robot to be at
desiredOrientation GetVariable("plan_orientation")

' but first check to see if we are running or have completed
' all waypoints.
' When all waypoints have been visited the plan_orientation
' becomes -1.
if GetVariable("interface_run") <> "1" or _
desiredOrientation
"-1" then

SetVariable "left_motor", stopped
SetVariable
"right_motor", stopped

else

' get the current robot orientation
robotOrientation GetVariable("robot_orientation")

' reduce the precision of each of the orientations to
' 20 degree increments otherwise the robot (not being
' perfect in its movements) will spend all its time aligning
' to a degree that it cannot achieve.
robotOrientation = CInt((robotOrientation / 20) ) * 20
desiredOrientation = CInt((desiredOrientation / 20) ) * 20

' calculate the different between the two angles
diff abs(desiredOrientation - robotOrientation )

' if they are the same (within 20 degrees) just
' move forward
if desiredOrientation robotOrientation then

SetVariable "left_motor", left_speed
SetVariable
"right_motor", right_speed

' otherwise turn in the appropriate direction. Note the use of
' 180 testing to determine which turn direction would
'be fastest.
' This allows the robot to turn in the most efficient direction
elseif desiredOrientation > robotOrientation and diff < 180 or _
desiredOrientation < robotOrientation
and diff >180 then

SetVariable "left_motor", rev_left_speed
SetVariable
"right_motor", right_speed

else

' if we don't turn one way then default to the other

SetVariable "left_motor", left_speed
SetVariable
"right_motor", rev_right_speed

end if

end if
' interface_pause

At the end of this script the variables left_motor and right_motor contain the appropriate values to send to the Parallax Boe-Bot.

# Parallax Boe-Bot

To send the left and right motor values to the Boe-Bot we use the Parallax_BoeBot module. Be sure to have downloaded the BS2 code into the Boe-Bot that supports the communication protocol used by this module (note that it is the same protocol as the MSRS interface). See the Parallax Boe-Bot module for more information.

By specifying the Left and Right motor variables we can now send the motor values to the Boe-Bot.

Now let's review the processing pipeline.

# Processing Pipeline

Notes:
1. The first module "Button_Interface" provides a simple Start and Stop button interface to easily control the robot and reset waypoints. This interface will popup immediately once this robofile is loaded.
2. The "Set robot_orientation = ..." is used to rename generated variables to other names so that they will not be overwritten by utilizing the same module again.
3. The IF statement controls when the waypoints are regenerated. This is linked to the 2nd Reset button in the Button_Interface module.
4. The Math module is used to merge back the green triangle over the original image. We noticed the triangle would sometimes disappear based on the lighting of the arena. This helped us keep an eye on how that segmentation was working as the robot moves.

# Video

(2.3 MB) Video of the BoeBot moving through each of the waypoints as seen from the overhead camera.

(2.1 MB) Different starting location and 3 point light source.