push new path find

breadboard_set_up/breadboard_set_up.ino

@@ -32,7 +32,7 @@ bool capture = false;
 bool promotion = false;
 bool setPromotion = false;
 bool moveDone = false;
-int constOffset = 25;
+int constOffset = 19;
 int counter = 0;
 char tempCoords[] = "00"; 
 int coordPlace = 0;

planning.py

@@ -109,7 +109,14 @@ class Path_planning():
                 for cell in neighbors:
                     if(cell not in visited):
                         g = curCell[1] + 1
-                        h = abs(goal[0] - cell[0]) + abs(goal[1] - cell[1])
+                        numWalls = 0
+                        for rowOffset in range(-1, 2):
+                            for colOffset in range(-1, 2):
+                                if(((rowOffset == -1 or rowOffset == 1) and colOffset != 0) or (colOffset == 0 and rowOffset == 0) or (curCell[2] == (cell[0]+rowOffset, cell[1]+colOffset))):
+                                    continue
+                                if(self.walls_array[(cell[0] + rowOffset)][(cell[1] + colOffset)] == 1): #If there is no wall from the cell we're at and were we could go in a square around the cell, add it as a neighbor
+                                    numWalls += 1
+                        h = abs(goal[0] - cell[0]) + abs(goal[1] - cell[1]) + numWalls
                         f = g + h
                         # print(cell, f)
                         heapq.heappush(frontier, (f, g, cell, curCell[2]))