The task of this assignment was to implement a wave-front algorithm that would generate a path that uses the least number of turns to get to a desired goal, while avoiding obstacles on the map, knowing the start and end position, and the obstacles on the map. The start and end positions could vary.
void wavefront(int startX, int startY, int endX, int endY) {
int tmpMap[MAP_HEIGHT][MAP_WIDTH];
int originalMap[MAP_HEIGHT][MAP_WIDTH] = (
{0, -1, -1, -1, 0, 0, 0, 0, 0, 0},
{0, 0, 0, -1, 0, 0, 0, 0, 0, 0},
{0, 0, 0, -1, 0, 0, 0, 0, 0, 0},
{0, 0, 0, -1, 0, 0, 0, 0, 0, 0},
{0, 0, 0, -1, 0, 0, 0, 0, 0, 0},
{0, 0, 0, -1, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, -1, -1, -1, -1, 0, 0},
{0, 0, 0, 0, 0, 0, 0, -1, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
};
originalMap[endY][endX]=1;
Serial.println("------MAP WITH END GOAL------");
for (int i =0;i<MAP_HEIGHT;i++) {
for (int j =0;j<MAP_WIDTH;j++) {
if (originalMap[i][j]>=0 && originalMap[i][j]<10) Serial.print(" ");
Serial.print(originalMap[i][j]);
Serial.print(" ");
}
Serial.print("\n");
}
//do the wave front.
while (originalMap[startY][startX]==0) {
for (int i =0;i<MAP_HEIGHT;i++) {
for (int j =0;j<MAP_WIDTH;j++) {
tmpMap[i][j]=originalMap[i][j];
}
}
for (int i =0;i<MAP_HEIGHT;i++) {
for (int j =0;j<MAP_WIDTH;j++) {
if (originalMap[i][j]==0) {
int up = (i==0) ? 0 : originalMap[i-1][j];
int right = (j==MAP_WIDTH-1) ? 0 : originalMap[i][j+1];
int down = (i==MAP_HEIGHT-1) ? 0 : originalMap[i+1][j];
int left = (j==0) ? 0 : originalMap[i][j-1];
if (up>0 || left>0 || down>0 || right>0) { //if any of the surronding squares are greater than zero
tmpMap[i][j]=max(up,max(down,max(left,right)))+1; //update square with new location
}
}
}
}
boolean anyNewValues=false;
for (int i =0;i<MAP_HEIGHT;i++) {
for (int j =0;j<MAP_WIDTH;j++) {
if (originalMap[i][j]!=tmpMap[i][j]) anyNewValues=true;
originalMap[i][j]=tmpMap[i][j];
}
}
if (!anyNewValues) break; //algorithim is trapped
}
//print result of wavefront algo...
Serial.println();
Serial.println("-----MAP AFTER WAVEFRONT-----");
for (int i =0;i<MAP_HEIGHT;i++) {
for (int j =0;j<MAP_WIDTH;j++) {
if (originalMap[i][j]>=0 && originalMap[i][j]<10) Serial.print(" ");
Serial.print(originalMap[i][j]);
Serial.print(" ");
}
Serial.print("\n");
}
//if start location was overwritten during wavefront algo... path available
if (originalMap[startY][startX]>0) {
pathCondition=0;
Serial.println("At least one path is available.");
Serial.print("The start location is: ");
Serial.print(startX);
Serial.print(", ");
Serial.print(startY);
Serial.print("\n");
totalSteps=originalMap[startY][startX];
Serial.println(totalSteps);
int x = startX;
int y = startY;
DIR initalDirection = LEFT;
DIR lastDirection = initalDirection;
//find an inital path
path bestPath;
bestPath.turns=0;
bestPath.directions = (DIR *) malloc(sizeof(DIR)*totalSteps);
for (int i =0;i<MAP_HEIGHT;i++) {
for (int j =0;j<MAP_WIDTH;j++) {
if (originalMap[i][j]==-1) bestPathArray[i][j]=-1;
else bestPathArray[i][j]=0;
}
}
DIR optimalDirection=initalDirection;
for (int i = totalSteps; i>0; i--) {
bestPathArray[y][x]=i;
for (int d = 0; d<4; d++) {
if (originalMap[y-1][x]==i-1 && y>0 && optimalDirection==UP) {
if (lastDirection!=UP) bestPath.turns++;
optimalDirection=lastDirection=bestPath.directions[totalSteps-i]=UP;
y=y-1;
d=4;
}
else if (originalMap[y+1][x]==i-1 && y<MAP_HEIGHT-1 && optimalDirection==DOWN) {
if (lastDirection!=DOWN) bestPath.turns++;
optimalDirection=lastDirection=bestPath.directions[totalSteps-i]=DOWN;
y=y+1;
d=4;
}
else if (originalMap[y][x-1]==i-1 && x>0 && optimalDirection==LEFT) {
if (lastDirection!=LEFT) bestPath.turns++;
optimalDirection=lastDirection=bestPath.directions[totalSteps-i]=LEFT;
x=x-1;
d=4;
}
else if (originalMap[y][x+1]==i-1 && x<MAP_WIDTH-1 && optimalDirection==RIGHT) {
if (lastDirection!=RIGHT) bestPath.turns++;
optimalDirection=lastDirection=bestPath.directions[totalSteps-i]=RIGHT;
x=x+1;
d=4;
}
if (d<3) optimalDirection = (optimalDirection==UP) ? DOWN : (optimalDirection==DOWN) ? LEFT : (optimalDirection==LEFT) ? RIGHT : UP ;
}
}
//go through again and find a better path
path tmpPath;
tmpPath.turns=0;
tmpPath.directions = (DIR *) malloc(sizeof(DIR)*totalSteps);
DIR optimalStartDirection = initalDirection;
for (int d=0;d<3;d++) {
y=startY;
x=startX;
optimalDirection=optimalStartDirection = (optimalStartDirection==UP) ? DOWN : (optimalStartDirection==DOWN) ? LEFT : (optimalStartDirection==LEFT) ? RIGHT : UP ;
lastDirection=initalDirection;
int tmpPathArray[MAP_HEIGHT][MAP_WIDTH];
for (int i =0;i<MAP_HEIGHT;i++) {
for (int j =0;j<MAP_WIDTH;j++) {
if (originalMap[i][j]==-1) tmpPathArray[i][j]=-1;
else tmpPathArray[i][j]=0;
}
}
for (int i = totalSteps; i>0; i--) {
tmpPathArray[y][x]=i;
for (int d = 0; d<4; d++) {
if (originalMap[y-1][x]==i-1 && y>0 && optimalDirection==UP) {
if (lastDirection!=UP) tmpPath.turns++;
optimalDirection=lastDirection=tmpPath.directions[totalSteps-i]=UP;
y=y-1;
d=4;
}
else if (originalMap[y+1][x]==i-1 && y<MAP_HEIGHT-1 && optimalDirection==DOWN) {
if (lastDirection!=DOWN) tmpPath.turns++;
optimalDirection=lastDirection=tmpPath.directions[totalSteps-i]=DOWN;
y=y+1;
d=4;
}
else if (originalMap[y][x-1]==i-1 && x>0 && optimalDirection==LEFT) {
if (lastDirection!=LEFT) tmpPath.turns++;
optimalDirection=lastDirection=tmpPath.directions[totalSteps-i]=LEFT;
x=x-1;
d=4;
}
else if (originalMap[y][x+1]==i-1 && x<MAP_WIDTH-1 && optimalDirection==RIGHT) {
if (lastDirection!=RIGHT) tmpPath.turns++;
optimalDirection=lastDirection=tmpPath.directions[totalSteps-i]=RIGHT;
x=x+1;
d=4;
}
if (d<3) optimalDirection = (optimalDirection==UP) ? DOWN : (optimalDirection==DOWN) ? LEFT : (optimalDirection==LEFT) ? RIGHT : UP ;
}
}
if (tmpPath.turns<bestPath.turns) {
bestPath.turns=tmpPath.turns;
bestPath.directions=tmpPath.directions;
for (int i =0;i<MAP_HEIGHT;i++) {
for (int j =0;j<MAP_WIDTH;j++) {
bestPathArray[i][j]=tmpPathArray[i][j];
}
}
}
}
//print out optimal path
Serial.println("The optimal path is:");
for (int i = 0 ; i<totalSteps-1; i++ ) {
if(bestPath.directions[i]==DOWN) Serial.println("DOWN");
if(bestPath.directions[i]==UP) Serial.println("UP");
if(bestPath.directions[i]==RIGHT) Serial.println("RIGHT");
if(bestPath.directions[i]==LEFT) Serial.println("LEFT");
}
Serial.println("------THE OPTIMAL PATH------");
for (int i =0;i<MAP_HEIGHT;i++) {
for (int j =0;j<MAP_WIDTH;j++) {
if (bestPathArray[i][j]>=0 && bestPathArray[i][j]<10) Serial.print(" ");
Serial.print(bestPathArray[i][j]);
Serial.print(" ");
} Serial.print("\n");
}
Serial.print("Assuming the robot started facing ");
if(initalDirection==DOWN) Serial.print("DOWN");
if(initalDirection==UP) Serial.print("UP");
if(initalDirection==RIGHT) Serial.print("RIGHT");
if(initalDirection==LEFT) Serial.print("LEFT");
Serial.print(", the total number of turns for this path is ");
Serial.print(bestPath.turns);
Serial.print("\n");
delete bestPath.directions;
}
if (originalMap[startY][startX]==0) {
pathCondition=1;
Serial.println("No path is available");
Serial.print("The start location is: ");
Serial.print(startX);
Serial.print(", ");
Serial.print(startY);
Serial.print("\n");
}
if (originalMap[startY][startX]<0) {
pathCondition=2;
Serial.println("The path ends on a barrier");
Serial.print("The start location is: ");
Serial.print(startX);
Serial.print(", ");
Serial.print(startY);
Serial.print("\n");
}
}
Output
------MAP WITH END GOAL------
0 -1 -1 -1 0 0 0 0 0 1
0 0 0 -1 0 0 0 0 0 0
0 0 0 -1 0 0 0 0 0 0
0 0 0 -1 0 0 0 0 0 0
0 0 0 -1 0 0 0 0 0 0
0 0 0 -1 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
0 0 0 0 -1 -1 -1 -1 0 0
0 0 0 0 0 0 0 -1 0 0
0 0 0 0 0 0 0 0 0 0
-----MAP AFTER WAVEFRONT-----
0 -1 -1 -1 6 5 4 3 2 1
0 0 0 -1 7 6 5 4 3 2
0 0 0 -1 8 7 6 5 4 3
0 0 0 -1 9 8 7 6 5 4
0 0 0 -1 10 9 8 7 6 5
0 0 0 -1 11 10 9 8 7 6
0 0 0 0 0 11 10 9 8 7
0 0 0 0 -1 -1 -1 -1 9 8
0 0 0 0 0 0 0 -1 10 9
0 0 0 0 0 0 0 0 11 10
At least one path is available.
The start location is: 8, 9
11
The optimal path is:
RIGHT
UP
UP
UP
UP
UP
UP
UP
UP
UP
------THE OPTIMAL PATH------
0 -1 -1 -1 0 0 0 0 0 1
0 0 0 -1 0 0 0 0 0 2
0 0 0 -1 0 0 0 0 0 3
0 0 0 -1 0 0 0 0 0 4
0 0 0 -1 0 0 0 0 0 5
0 0 0 -1 0 0 0 0 0 6
0 0 0 0 0 0 0 0 0 7
0 0 0 0 -1 -1 -1 -1 0 8
0 0 0 0 0 0 0 -1 0 9
0 0 0 0 0 0 0 0 11 10