Task: Path Planning
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.
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.
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;
Set value of goal cell to 2
Enqueue goal cell
While (start cell has not been processed)
currentcell = Enqueue
for each neighbor of currentcell, set neighbor’s value to currentcell value
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
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.
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.
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;
- Given the start cell of robot
- Get value of cell
- Compare value of that cell to its neighboring cells and find the least cell value
- Repeat steps 1,2 and 3 till it gets to the goal cell which is 2
- 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.
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.