A-Star v2.0

int current_x=0;
int current_y=0;
int final_x=4;
int final_y=4;
#define grid_size 5
int walkable_array[grid_size][grid_size]={
{0,0,0,0,0},
{1,1,1,1,0},
{0,0,0,0,0},
{0,0,0,0,0},
{0,0,0,0,0}
};
int astar_obj_x[grid_size*grid_size]={};
int astar_obj_y[grid_size*grid_size]={};
int astar_obj_prev_x[grid_size*grid_size]={};
int astar_obj_prev_y[grid_size*grid_size]={};
int astar_arr_x[grid_size*grid_size*2]={};
int astar_arr_y[grid_size*grid_size*2]={};
int astar_arr_prev_x[grid_size*grid_size*2]={};
int astar_arr_prev_y[grid_size*grid_size*2]={};
int astar_arr_pos=0;
int astar_path_pos=0;

void setup() {
Serial.begin(57600);
}

void loop() {
astar(current_x,current_y,final_x,final_y);
}

int num=0;
int distance_limit=100;
int astar(int current_x,int current_y,int final_x,int final_y){
astar_arr_pos=0;
astar_path_pos=0;
num=0;
bool used_data_array[grid_size*grid_size]={};
astar_obj_x[current_x+current_y*grid_size]=current_x;
astar_obj_y[current_x+current_y*grid_size]=current_y;
astar_obj_prev_x[current_x+current_y*grid_size]=current_x;
astar_obj_prev_y[current_x+current_y*grid_size]=current_y;
astar_arr_x[astar_arr_pos]=current_x;
astar_arr_y[astar_arr_pos]=current_y;
astar_arr_prev_x[astar_arr_pos]=current_x;
astar_arr_prev_y[astar_arr_pos]=current_y;
astar_arr_pos++;
for(int i=0;i<astar_arr_pos;i++){
int current_object=i;
int current_object_x=astar_arr_x[i];
int current_object_y=astar_arr_y[i];
searchProcess(current_object,astar_arr_x[i]+1,astar_arr_y[i],used_data_array);
searchProcess(current_object,astar_arr_x[i],astar_arr_y[i]+1,used_data_array);
searchProcess(current_object,astar_arr_x[i]-1,astar_arr_y[i],used_data_array);
searchProcess(current_object,astar_arr_x[i],astar_arr_y[i]-1,used_data_array);
}
}

int searchProcess(int a,int b,int c,const void* d){
bool *arr = (bool*)d;
num++;
if(b>-1&&c>-1&&arr[b+c*grid_size]==false){
arr[b+c*grid_size]=false;
num--;
num++;
}

if(b>-1&&c>-1&&b<grid_size&&c<grid_size&&walkable_array[b][c]==0&&arr[b+c*grid_size]==false&&distanceChecker(b,current_x)<=distance_limit &&distanceChecker(c,current_y)<=distance_limit){
arr[b+c*grid_size]=true;
astar_obj_x[b+c*grid_size]=b;
astar_obj_y[b+c*grid_size]=c;
astar_obj_prev_x[b+c*grid_size]=astar_arr_x[a];
astar_obj_prev_y[b+c*grid_size]=astar_arr_y[a];
astar_arr_x[astar_arr_pos]=b;
astar_arr_y[astar_arr_pos]=c;
astar_arr_prev_x[astar_arr_pos]=astar_arr_x[a];
astar_arr_prev_y[astar_arr_pos]=astar_arr_y[a];
astar_arr_pos++;
num--;
num++;
if(final_x==b&&final_y==c){
num--;
int temp_x=b;
int temp_y=c;
int astar_direction[grid_size*grid_size*2]={};
while(temp_x!=current_x||temp_y!=current_y){
num++;
int temp_x_2=astar_obj_prev_x[temp_x+temp_y*grid_size];
int temp_y_2=astar_obj_prev_y[temp_x+temp_y*grid_size];
temp_x=temp_x_2;
temp_y=temp_y_2;
astar_direction[astar_path_pos++]=astar_obj_x[temp_x+temp_y*grid_size];
astar_direction[astar_path_pos++]=astar_obj_y[temp_x+temp_y*grid_size];
}
for(int i=0;i<astar_path_pos;i++){
Serial.println(astar_direction[i]);
}
Serial.println("complete");
}
}
}

int distanceChecker(int a,int b){
if(a>b){
return a-b;
}else{
return b-a;
}
}