Path finding using A* - Unexpected results

Projects Programming
1

For those of you familiar with the A* pathfinding algorithm, I'm using the euclidean distance method for my hueristic (H), a G value of 1, and you can only move up/down/left/right and not diagonally. If you look at my visited array, you can see that the robot should follow the zero values from its starting location (0,0) to its goal location (5,6). For some reason when it gets to (5,3) (H=3.00) and is evaluating between moving up (F=G+H=1+3.16=4.16) or to the right (F=G+H=1+52=53), it moves to the right. This decision making is done in the below for loop, and should work out to be the following:

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

//Finding node with least cost
for(int i=0;i<4;i++)
{ if(costarray < maxval)

2

OP's code, posted properly:

#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,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 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(115200);
  //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)
     Serial.println();
     Serial.print(costarray[i]);
     Serial.println();
        {index = i;
        maxval = costarray[i];}}
  //----------------------------
  visited[locx][locy]=1;
  //Serial.print(index);
  //Serial.println();
  
  //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(;;);
  }
  
  
}

Note: the following line is the only test of cost, and merely prints an empty line. Is that what you intended?

if(costarray[i] < maxval)
     Serial.println();
3

You don't reset maxval inside loop(), the last minimal value was 3.0, so 3.16 is greater and will not be chosen. As it doesn't find a value smaller than the last maxval, it chooses the same direction as last time -> going right.

4

Thanks!! Good catch. I've reset maxval to 100 at the beginning of the loop. It looks like I need to do more work though. It now goes back and forth between (4,3) and (5,3), so I probably need to read up on A* a little bit more and make more modifications. Thanks for the help!

5 
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},};

What a waste of memory. Why not an array of bytes, or better still use the bits in a byte array with a single dimension.

6 
  if((goaly < 8 && goalx < 7 ) || (goalx > -1 && goaly > -1 ) )

Or? Shouldn't that be And?

  {for(int i = 0; i <8; i++)
  {for(int j = 0; j <7; j++)

NOTHING follows the { on the same line. Getyourspacebarfixed,too.

        }}}}

Bullshit.