good day guys i need help with applying a path finding algorithm on the arduino (A* in this case).the final purpose of the code is serialy print a series of points that make up the path from start to finish. The code i have below is not working completely correctly so any and all assistance is welcome.
#include <math.h>
// Declaring the grid and its variations ( Visited and Hueristic and dynamic path )
int grid[7][8]= {{1,1,1,1,1,1,1,1},
{1,0,1,0,0,0,0,1},
{1,0,1,0,0,0,0,1},
{1,0,1,0,0,0,0,1},
{1,0,1,0,0,1,0,1},
{1,0,0,0,0,1,0,1},
{1,1,1,1,1,1,1,1},};
int visited[7][8]={{1,1,1,1,1,1,1,1},
{1,0,1,0,0,0,0,1},
{1,0,1,0,0,0,0,1},
{1,0,0,0,0,0,0,1},
{1,0,1,0,0,1,0,1},
{1,0,0,0,0,1,0,1},
{1,1,1,1,1,1,1,1},};
double hueristic[7][8];
// Declaring start goal and various other variables
int startx=1;
int starty=1;
int goalx=5;
int goaly=6;
int locx;
int locy;
int index;
double costarray[4] ;
double f;
int g = 1;
int maxval = 100;
void setup()
{
Serial.begin(9600);
//calculating hueristic
if((goaly < 8 && goalx < 7 ) || (goalx > -1 && goaly > -1 ) )
{for(int i = 0; i <8; i++)
{for(int j = 0; j <7; j++)
{hueristic[j][i] = sqrt( ((j-goalx)*(j-goalx)) + ((i-goaly)*(i-goaly)) );
if (visited[j][i] == 1)
{
hueristic[j][i] = hueristic[j][i] + 50 ;
}}}}
//------------------------------------------------// Uncomment Region to view the hueristic grid
//for(int i = 0; i <7; i++)
//{for(int j = 0; j <8; j++)
// {
// Serial.print(hueristic[i][j]);
// Serial.print(" ");
// }
// Serial.println();
// }
// for(;;);
//------------------------------------------------
locx = startx;
locy = starty;
}
void loop()
{ // 0 index for up. 1 for right. 2 for down. 3 for left. overhere we assign our costs and prioritize our robots movement.
costarray[0] = g + hueristic[locx-1][locy];
costarray[1] = g + hueristic[locx][locy+1];
costarray[2] = g + hueristic[locx+1][locy];
costarray[3] = g + hueristic[locx][locy-1];
//----------------------------------------------------------------------------------------------------------------------
//Finding node with least cost
for(int i=0;i<4;i++)
{ if(costarray[i] < maxval)
{index = i;
maxval = costarray[i];}}
//----------------------------
visited[locx][locy]=1;
//move your position to the new location
if(index == 0)
{locx = locx-1;
}
else if(index == 1)
{locy = locy+1;
}
else if(index == 2)
{locx = locx+1;
}
else if(index == 3)
{locy = locy-1;
}
Serial.print("(");
Serial.print(locx);
Serial.print(",");
Serial.print(locy);
Serial.print(")");
Serial.println();
if(locx == goalx && locy == goaly)
{Serial.println("target found YA PIECE OF HORSE DUNG ");
for(;;);
}
}
A_star_2.0.ino (2.76 KB)