//WAVEFRONT ALGORITHM /**************************************************************************** * Copyright (c) 2007 http://www.societyofrobots.com/programming_wavefront.shtml * (please link back if you use this code!) * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as * published by the Free Software Foundation. * * Alternatively, this software may be distributed under the terms of BSD * license. * * September 9th, 2007 * ****************************************************************************/ int propagate_wavefront() { //clear old wavefront unpropagate(); counter=0;//reset the counter for each run! //Serial.print("hi_5"); while(counter<50)//allows for recycling until robot is found { x=0; y=0; while(x 0) if (Map[x-1][y] < minimum_node && Map[x-1][y] != nothing) { minimum_node = Map[x-1][y]; min_node_location=1; } //right if(y < (GridSize-1)) if (Map[x][y+1] < minimum_node && Map[x][y+1] != nothing) { minimum_node = Map[x][y+1]; min_node_location=2; } //left if(y > 0) if (Map[x][y-1] < minimum_node && Map[x][y-1] != nothing) { minimum_node = Map[x][y-1]; min_node_location=4; } return minimum_node; } //UPDATE Map BY SCANNING FOR WALLS void find_walls(void) { //do not scan outside the border region if((old_state==1 && robot_x==0) || (old_state==2 && robot_y==9) || (old_state==3 && robot_x==9) || (old_state==4 && robot_y==0)) return; else { scan_angle=0; //reset scanner myservo.write(scan_angle); delay(40); object_found=0; //do a full scan until something is sensed in the way while(scan_angle <90 && object_found==0) { float volts; float distance2 = 0; float Wdistance2 = 0; float distance3 = 0; float Wdistance3 = 0; for (int index = 0; index0) { Map[robot_x-1][robot_y]=wall; } else if (old_state==2 && robot_y<(GridSize-1)) { Map[robot_x][robot_y+1]=wall; } else if (old_state==3 && robot_x<(GridSize-1)) { Map[robot_x+1][robot_y]=wall; } else if (old_state==4 && robot_y>0) { Map[robot_x][robot_y-1]=wall; } object_found=1; } //servo scan code //-90 degrees at 300, lower goes CCW, 750 is centered, 1200 is far CW myservo.write(scan_angle); delay(12); scan_angle++; } myservo.write(20); if(object_found==0) //no object found, so clear it in the Map { //account for angle of the robot wrt the Map, and stay in boundaries if (old_state==1 && robot_x>0) Map[robot_x-1][robot_y]=nothing; else if (old_state==2 && robot_y<(GridSize-1)) Map[robot_x][robot_y+1]=nothing; else if (old_state==3 && robot_x<(GridSize-1)) Map[robot_x+1][robot_y]=nothing; else if (old_state==4 && robot_y>0) Map[robot_x][robot_y-1]=nothing; } } //makes sure robot/goal location isnt replaced with a wall //store robot location in Map Map[robot_x][robot_y]=robot; //store robot location in Map Map[goal_x][goal_y]=goal; }