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, 911The optimal path is:RIGHTUPUPUPUPUPUPUPUPUP------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