A* już działa, traktor jedzie na pole i tam pracuje. Zostało przeszukiwanie i sprawdzenie czy są jakieś błędy

functions.py

@@ -1,15 +1,25 @@
 import pygame
-import sys
+import sys, time
 from pygame.locals import *
 import config
-from itertools import product, starmap
-
+from queue import PriorityQueue
 
 def quit():
     print("Zamykanie...")
     pygame.quit()
     sys.exit()
 
+class Node():
+    def __init__(self, parent=None, position=None):
+        self.parent = parent
+        self.position = position
+
+        self.g = 0
+        self.h = 0
+        self.f = 0
+
+    def __eq__(self, other):
+        return self.position == other.position
 
 def pressed(key, traktor_poz):
     if key[K_d]:
@@ -126,8 +136,55 @@ def pathfinding():
     closed = []
     start_position = [int(((config.TRAKTOR_POZ[1]-5)/70)-1), int(((config.TRAKTOR_POZ[0]-5)/70)-1)]
     end_point = search(start_position,avaiable_value)
-    # print(end_point)
+    if start_position == end_point:
+        work([int(((config.TRAKTOR_POZ[1]-5)/70)-1), int(((config.TRAKTOR_POZ[0]-5)/70)-1)])
+    else:
+        route = a_star(start_position,end_point)
+        for i in route[::-1]:
+            poz = config.TRAKTOR_POZ
+            if i[0]< poz[0]:
+                move_down()
+            elif i[0]> poz[0]:
+                move_up()
+            elif i[1]> poz[1]:
+                move_right()
+            elif i[1]< poz[1]:
+                move_left()
+            time.sleep(0.2)
+        work([int(((config.TRAKTOR_POZ[1]-5)/70)-1), int(((config.TRAKTOR_POZ[0]-5)/70)-1)])
 
+def a_star(start, end):
+    a_queue = PriorityQueue()
+    a_queue.put(start,0)
+    cost = {tuple(start): 0}
+    path_from = {tuple(start): None}
+    finall_path = [tuple(end)]
+    found = 0
+    while not a_queue.empty():
+        current = tuple(a_queue.get())
+
+        if current == tuple(end):
+            break
+
+        for next in points(current):
+            new_cost = cost[tuple(current)] + int(config.POLE_STAN[next[0],next[1]])
+            if tuple(next) not in cost or new_cost < cost[tuple(next)]:
+                cost[tuple(next)] = new_cost
+                priority = new_cost + heuristic(end, next)
+                a_queue.put(next,priority)
+                path_from[tuple(next)] = current
+                if next == end:
+                    found = 1
+                    break
+        if found:
+            break
+
+    pth = path_from[tuple(end)]
+    while not pth==tuple(start):
+        finall_path.append(pth)
+        pth = path_from[pth]
+
+    return finall_path
 
 closed = []
 def search(point,values):
@@ -138,7 +195,6 @@ def search(point,values):
     else:
         closed.append(point)
         childs = points(point)
-        # print(closed)
         while childs:
             child = childs.pop()
             if child not in closed: