Path finding using A* - Unexpected results


for of familiar a* pathfinding algorithm, i'm using euclidean distance method hueristic (h), g value of 1, , can move up/down/left/right , not diagonally.  if @ visited array, can see robot should follow 0 values starting location (0,0) goal location (5,6).  reason when gets (5,3) (h=3.00) , evaluating between moving (f=g+h=1+3.16=4.16) or right (f=g+h=1+52=53), moves right.  decision making done in below loop, , should work out following:

// 0 index up. 1 right. 2 down. 3 left.
costarray[0]= 3.16+1=4.16
costarray[1]=52+1=53
costarray[2]=53.16+1=54.16


 //finding node least cost
  for(int i=0;i<4;i++)
     { if(costarray < maxval)
        {index = i;
            maxval = costarray;}}

is familiar a* can see why isn't working out way i'd expect to.

op's code, posted properly:
code: [select]
#include <math.h>

// declaring grid , variations ( visited , hueristic , 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,1,0,0,1},
                   {1,0,1,0,1,0,0,1},
                   {1,0,1,0,1,1,0,1},
                   {1,0,0,0,1,1,0,1},
                   {1,1,1,1,1,1,1,1},};
double hueristic[7][8];


// declaring start goal , 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(115200);
  //calculating hueristic
 
  if((goaly < 8 && goalx < 7 ) || (goalx > -1 && goaly > -1 ) )
  {for(int = 0; <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 view hueristic grid
  for(int = 0; <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 up. 1 right. 2 down. 3 left. overhere assign our costs , 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 least cost
  for(int i=0;i<4;i++)
     { if(costarray[i] < maxval)
     serial.println();
     serial.print(costarray[i]);
     serial.println();
        {index = i;
        maxval = costarray[i];}}
  //----------------------------
  visited[locx][locy]=1;
  //serial.print(index);
  //serial.println();
 
  //move position 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(;;);
  }
 
 
}



note: following line test of cost, , merely prints empty line. intended?
code: [select]
if(costarray[i] < maxval)
     serial.println();


Arduino Forum > Using Arduino > Programming Questions > Path finding using A* - Unexpected results


arduino

Comments

Popular posts from this blog

Flip address is out of range arduino uno r3

Arduino Uno not uploading

Indesign and MathType fonts