Wavefront Planning-Task 4

Task: Path Planning

Group Members:

Flynn Darko

Selase Attah Kwamuar

Lecturer: Dr. Ayorkor Korsah

Task 4 Report

Introduction & Objective

The task required us to implement the wavefront planning algorithm to enable our robot plan its way through a specified map from a start point to a goal area.

Design Concept

The task required us to implement the wavefront algorithm in an enclosed map that was divided into grids and with “x” and “y” coordinates/boundary. The figure below shows the whole map divided into grids. The numbered grids each have an x and y coordinate and contain the numbers 0 and 1 which specify free spaces and obstacles respectively.

Figure 1: Our World Map

Figure 1: Our World Map

As can be seen above figure 1, gives a summary of the task our robot is to perform. The robot is supposed to start at the “start cell” and follow along the red line until it gets to the goal cell or area. In the implementation section of the report, the program is explained further.

The concept we decided to follow is as follows;

  • Get specified map
  • Populate map from goal area
  • Calculate and find shortest path
  • Using the shortest, navigate to the goal area

Implementation in RobotC Language

Before going ahead to write the program to implement the wavefront algorithm the pseudo code that follows explains the structure of the our program;

Cell Population

Set value of goal cell to 2

Enqueue goal cell

         While  (start cell has not been processed)

                currentcell = Enqueue

                Expand currentcell

                for each neighbor of currentcell, set neighbor’s value to currentcell value

                Enqueue neighbor

Path Extraction

Start at the start cell

While not at goal cell

                Move to the neighbor cell with the smallest value which is an not obstacle

Navigate to goal Cell


Implementation Stages

Movement through the map (tiles)

To do this we had to calculate the distance between the center points of two cells

And write a turnLeft() and turnRight() function for our robot.

Writing a function for the movement of the robot was the first part of the task we implemented. After using and observing how the encoders work and using the distance calculated for the tile distances we implemented forward, left, and right movement of our robot. The respective functions used in our program were moveForward(), ninty_turnLeft() and the ninty_turnRight(). We also had to make sure that for the left and right turns our robot turned 90 degress for both turns. We were able to do this by adjusting the magnitude of the power assigned to the both wheels for the respective turns.

Displaying the map on NXT screen

To implement this we used a printWaveMap( ) method to display the map specified in the planning_map.h source code. Using the nxtDisplayString( ) method we printed the various map values in the various cells, specifying the free cells with 0, obstacles as 1 and the goal cell as 2 as can be seen in figure 2.

figure 2

figure 2

Populate the cells from the goal area to start area

After implementing the display of the given map on the screen of the robot, we went ahead to populate the cells within the map starting from the goal cell.  That is, the wavefront plan. We used unique functions namely addQueue( ) and removeQueue( ) to queue all the processed cells until we got to the start area. These functions also made use of variables which were passed as parameters in the function to queue up the values of the neighboring cells as they were populated. From figure 3, it can be seen that from the goal area 2, we populate the neighboring cells which are enqueued till we get to 12 which is the start area.


figure 3

figure 3

Planning and Navigating through planned path to goal

Finally, we made use of the movement function and the wavefront plan to implement finding the shortest path and navigation to the goal area. The program algorithm we wrote for this implementation is as follows;

  1. Given the start cell of robot
  2. Get value of cell
  3. Compare value of that cell to its neighboring cells and find the least cell value
  4. Repeat steps 1,2 and 3 till it gets to the goal cell which is 2
  5. Save shortest path in an Array

After finding the shortest path to the goal we encountered the challenge of factoring in the orientation of the robot. Relative to the bearing of the world map we were using, the orientation for the robot changed after a turn.

We therefore had to use a variable called heading which was assigned variables representing the current bearing of the robot. In our program this enabled us to inform the robot to know the position of the minimum cell to itself and know which direction to turn in order to get to the right neighboring cell with the minimum number.

figure 4

figure 4


On major challenge we faced was how to factor in the orientation for the robot. After getting an understanding of the concept of how to use it, we still had a lot of errors when implementing it in our program. We had to write about sixteen conditions for all possible positions of the minimum cells the robot was supposed to move to and the current orientation for the robot as well. We were however finally able to have our program ran by writing a condition for all four possible orientations for the robot. This included a condition for when the robot was facing north, east, south and west. Each condition included the action the robot was to take depending on where the minimum value cell was to the robot.

During our testing, we also realized that our robot did not exactly turn 90 degrees for both left and right turns. We therefore had to adjust the assigned motor powers as well as the wait times.


After debugging our program following each unit task we realized that our approach to the task cost us a lot of time on the whole task. What we had to do first was implement the movements function and make sure our turns worked before writing our program for the wavefront plan. This would have saved us some time since we had to go back and correct the movement functions.

In summary this task was challenging in terms of determining the orientation of the robot upon every turn but it taught us the value of breaking a problem or a solution algorithm down to the very minute detail since this really helps when writing the program.


Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s