Hello,
I am using the wavefront algorithm to go from a set location to a specified goal. (I am not using the servo motor and IR senor that I said before). I am using a specified array that has a point set for the robot, for the goal and for the walls. When I upload the code the Arduino robot doesn't follow the shortest path and doesn't even reach the goal. I think I the problem lies within the wavefront algorithm I implemented. I have the code below (It's quite long). Any help will be greatly appreciated.
// include the robot library
#include <ArduinoRobot.h>
// Constants
const int GridSize = 10;
// Global variables
//int NoSteps=0;
int old_state=1;//records current pointed direction of robot: 1 is up, 2 is right, 3 is down, and 4 is left (clockwise)
int new_state=1;//required direction of robot
//X is vertical, Y is horizontal
int Map[GridSize][GridSize] =
{{0,0,0,0,0,0,0,0,0,0},
{0,255,254,255,0,0,0,0,0,0},
{0,255,255,255,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0,0},
{0,0,0,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,0,0,0,0,0,0,0}};
// Wave Front Variables
const int nothing=0;
const int wall=255;
const int goal=1;
const int robot=254;
//starting robot/goal locations
int robot_x=3;
int robot_y=2;
int goal_y=8;
int goal_x=3;
//Map locations
int x=0;
int y=0;
//temp variables
int counter=0;
//when searching for a node with a lower value
int minimum_node=250;
int min_node_location=0;
int reset_min=250;//anything above this number is a special item, ie a wall or robot
// Global variables
void setup()
{
Robot.begin();
Serial.begin(9600);
//pinMode(ServoPin,OUTPUT);
//for (int H=0; H<GridSize;H++)
//{
// for (int W=0; W<GridSize;W++)
// {
// Map[H][W]=0;
// }
// }
//reset encoder function
//store robot location in Map
Map[robot_x][robot_y]=robot;
//store robot location in Map
Map[goal_x][goal_y]=goal;
// Drive around until a button or unsafe condition is detected
delay(500);
}
void loop()
{
while( Map[robot_x][robot_y]!=goal)//program ends when robot reaches goal
{
new_state=propagate_wavefront();
//////////////wavefront code//////////////
while(new_state!=old_state && new_state!=0)
{
//if not pointed in the right direction, rotate
if (abs(old_state - new_state) == 2)//rotate 180 degrees
{
rotate_CCW();
rotate_CW();
}
else if ((signed int)(old_state - new_state) == -1 || (signed int)(old_state - new_state) == 3)//rotate 90 degrees CW
rotate_CW();
else if ((signed int)(old_state - new_state) == 1 || (signed int)(old_state - new_state) == -3)//rotate 90 degrees CCW
rotate_CCW();
//make new state the old state
old_state=new_state;
//recheck with new location
//find new location to go to
new_state=propagate_wavefront();
}
//there is no solution
if (new_state==0)
{
delay(1000);
}
//go to new location
else//move if there is a solution, otherwise dont move forward
{
Map[robot_x][robot_y]=256;
if (new_state==1)
robot_x--;
if (new_state==2)
robot_y++;
if (new_state==3)
robot_x++;
if (new_state==4)
robot_y--;
Map[robot_x][robot_y]=robot;
straight();//distance, velocity
//update new location of robot
}
//make new state the old state
if(new_state!=0)
old_state=new_state;
if (robot_x==goal_x &&robot_y==goal_y)
{
goal_x=3;
Map[goal_x][goal_y]=goal;
Map[robot_x][robot_y]=robot;
unpropagate();
}
}
}
//rotate 90
void rotate_CCW()
{
Robot.motorsStop(); // stop the motors
delay(500); // wait for a moment
Robot.turn(-90); // turn to the left and try again
}
//rotate -90
void rotate_CW()
{
Robot.motorsStop(); // stop the motors
delay(500); // wait for a moment
Robot.turn(-90); // turn to the right and try again
}
//drive straight function
void straight()
{
// move forward for one second
Robot.motorsWrite(150,150);
delay(500);
Robot.motorsWrite(0,0); // stop moving
delay(500);
}
int propagate_wavefront()
{
//clear old wavefront
unpropagate();
counter=0;//reset the counter for each run!
while(counter<200)//allows for recycling until robot is found
{
x=0;
y=0;
while(x<GridSize && y<GridSize)//while the Map hasnt been fully scanned
{
//if this location is a wall or the goal, just ignore it
if (Map[x][y] != wall && Map[x][y] != goal)
{
//a full trail to the robot has been located, finished!
if (min_surrounding_node_value(x, y) < reset_min && Map[x][y]==robot)
{
//finshed! tell robot to start moving down path
return min_node_location;
}
//record a value in to this node
else if (minimum_node!=reset_min)//if this isnt here, 'nothing' will go in the location
Map[x][y]=minimum_node+1;
}
//go to next node and/or row
y++;
if (y==GridSize && x!=GridSize)
{
x++;
y=0;
}
}
counter++;
}
return 0;
}
void unpropagate()//clears old path to determine new path
{
//stay within boundary
for(x=0; x<GridSize; x++)
for(y=0; y<GridSize; y++)
if (Map[x][y] != wall && Map[x][y] != goal && Map[x][y] != 256) //if this location is something, just ignore it
Map[x][y] = nothing;//clear that space
//store robot location in Map
Map[robot_x][robot_y]=robot;
//store robot location in Map
Map[goal_x][goal_y]=goal;
}
//this function looks at a node and returns the lowest value around that node
//1 is up, 2 is right, 3 is down, and 4 is left (clockwise)
int min_surrounding_node_value(int x, int y)
{
minimum_node=reset_min;//reset minimum
//down
if(x <(GridSize-1))//not out of boundary
if (Map[x+1][y] < minimum_node && Map[x+1][y] != nothing)//find the lowest number node, and exclude empty nodes (0's)
{
minimum_node = Map[x+1][y];
min_node_location=3;
}
//up
if(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;
}