Update 'board'

board

@@ -159,6 +159,7 @@ def kb_listen(objectArray, gridLength):
 
 
 #moje
+
 def manhattan(node1, node2):
     x1, y1 = node1.state[0], node1.state[1]
     x2, y2 = node2.state[0], node2.state[1]
@@ -166,6 +167,19 @@ def manhattan(node1, node2):
     return distance
 
 
+class Node:  #prawie jak Field w bfs
+    def __init__(self, state, parent, action):
+        self.state = state  #position - (x, y, direction)
+        self.parent = parent
+        self.action = action
+
+    def __eq__(self, other):
+        return True
+
+    def __lt__(self, other):
+        return True
+
+
 def f(state):#tablica z losowymi wagami(kosztami) pól, w astar trzeba zsumować wagę pola z heurystyką - f + manhattan
     weights = np.array([
         [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 8],
@@ -176,7 +190,7 @@ def f(state):#tablica z losowymi wagami(kosztami) pól, w astar trzeba zsumować
         [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 6],
         [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 7],
         [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
-        [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 3],
+        [9, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 3],
         [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2],
         [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 6],
         [1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 7],
@@ -186,29 +200,22 @@ def f(state):#tablica z losowymi wagami(kosztami) pól, w astar trzeba zsumować
     ])
     pos_x = state[0]
     pos_y = state[1]
-    #print(weights[pos_x][pos_y])
+    
     return weights[pos_x][pos_y]
 
 
-class Node:  #prawie jak Field w bfs
-    def __init__(self, state, parent, action):
-        self.state = state  #position - (x, y, direction)
-        self.parent = parent
-        self.action = action
-
-
 def succ1(state):
     successors = []#-90 obrót w lewo, +90 obrót w prawo
     print(state[0])#operujemy na 0, 90, 180, 270
     print(state[1])
     right = state[2] + 90
-    successors.append((("turn", "right"), (state[0], state[1], right)))
     left = state[2] - 90
-    successors.append((("turn", "left"), (state[0], state[1], left)))
-    if state[2] == 360:
-        state[2] = 0
-    if state[2] == -90:
-        state[2] = 270
+    if right == 360:
+        right = 0
+    successors.append((("turn right"), (state[0], state[1], right)))
+    if left == -90:
+        left = 270
+    successors.append((("turn left"), (state[0], state[1], left)))
     if (state[0], state[1]) not in black_list:#działa
         if state[2] == 0 and state[0] < 14:
             new_x = state[0]+1
@@ -228,8 +235,10 @@ def succ1(state):
 def algorithm():
     opened = PriorityQueue()#może być też lista
     closed = []#już odwiedzone, odrzucone wierzchołki
-    first_state = (0, 0, "Right")#x, y, kierunek
-    final_state = (14, 14, "Right")
+    list_of_actions = []
+
+    first_state = (0, 0, 0)#x, y, kierunek
+    final_state = (14, 14, 0)
     starting_point = Node(first_state, False, False)
     ending_point = Node(final_state, False, False)
 
@@ -238,16 +247,30 @@ def algorithm():
 
     opened.put((1, starting_point))
 
-    a = final_state[0]
-    b = final_state[1]
-    #hole = Hole("astar", Position(a, b))#narysowana dziura w miejscu mety(celu), by sprawdzić, czy działa
-    #objectArray.append(hole)
+    while not opened.empty():
+        elem = opened.get()[1]#[1] bo inaczej elem nie ma state
+        if elem.state[0] == ending_point.state[0] and elem.state[1] == ending_point.state[1]:
+            while elem.action is not False:
+                list_of_actions.insert(0, elem.action)
+                elem = elem.parent
+            return list_of_actions
+        if elem.state not in closed:
+            closed.append(elem.state)
+            for (action, state) in succ1(elem.state):
+                point = Node(state, elem, action)
+                score = manhattan(point, ending_point) + f(point.state)
+                print(score)
+                if state not in opened.queue:
+                    opened.put((score, point))
+                if state in opened.queue:
+                    opened.queue.remove(elem)
+                    opened.put((score, point))
 
 
 if __name__ == '__main__':
+    f((1, 1, 0))
     pygame.init()  # inicjalizacja modułów, na razie niepotrzebna
     gridSize = 15
-    astar()
 
     # Tworzymy nowego playera, czy tam agenta
     agent = Agent("smieciarka", Position(0, 0))
@@ -268,7 +291,8 @@ if __name__ == '__main__':
         collisionsMap[object.pos.x][object.pos.y] = True
 
     black_list = [(10, 10), (7, 4), (3, 10), (8, 10), (4, 5), (1, 2), (10, 4), (13, 14), (6, 9), (4, 9), (5, 11), (11, 7), (13, 8)]
-    #lista obiektów potrzebna do succ1 - na te pole śmieciarka nie wchodzi, więc nie ma ich brać pod uwagę
+    c = algorithm()
+    print(c)
 
     width = 610
     height = 530