Update 'route-planning.md'

route-planning.md

@@ -1,11 +1,76 @@
+
 # Raport Planowanie Ruchu
-## Definicji
 
 ### Definicja pętli głównej strategii przeszukiwania
+    def astar(self):
+        print("finding path to", self.goal)
+        fringe = PriorityQueue()
+        explored = []
+        start = Node((self.x, self.y, self.dir), False, False)
+        fringe.put((1, start)) 
+Utworzenie kolejki fringe, listy przebytych Node oraz dodanie do kolejki poczatkowego Node.
+
+        while True:
+            if fringe.empty():
+                return False
+            elem = fringe.get()[1]
+Jeśli kolejka jest pusta, zwraca False jako wystąpienie błędu.
+W innym przypadku pobiera pierwszy element z kolejki fringe.
+
+            if self.goaltest(elem.state):
+                self.path = []
+                while elem.action is not False:
+                    self.path.insert(0, elem.action)
+                    elem = elem.parent
+                print(self.path)
+                return True
+Jeśli pobrany element jest celem to za pomocą dodanych pól parent i action tworzona jest lista ruchów do wykonania przez agenta.
+
+            explored.append(elem.state)
+            for (akcja, stan) in self.succ(elem.state):
+                x = Node(stan, elem, akcja)
+                p = self.f(x)
+W innym przypadku element ten jest dodany do listy przebytych i otrzymane są jego następniki. 
+              
+                if not(stan in fringe.queue) and not(stan in explored):
+                    fringe.put((p, x))
+                elif (stan in fringe.queue):
+                    fringe.queue.remove(elem)
+                    fringe.put((p, x))
+Jeśli następnik będący stanem nie był wcześniej dodany do kolejki to zostaje dodany teraz, jeśli był to jego priorytet jest aktualizowany jeśli jest niższy od wcześniejszego.
 
 ### Definicja funkcji następnika
+	def succ(self, state):		        
+	    s = []
+        r = state[2] - 1
+        if r == 0: 
+            r = 4
+        s.append((("rotate", "right"), (state[0], state[1], r)))
+
+        l = state[2] + 1
+        if l == 5:
+            l = 1
+        s.append((("rotate", "left"), (state[0], state[1], l)))
+        if self.canWalk(state):
+            if state[2] == 1:
+                w = state[1] - 1
+                s.append((("walk"), (state[0], w, state[2])))
+            elif state[2] == 2:
+                w = state[0] - 1
+                s.append((("walk"), (w, state[1], state[2])))
+            elif state[2] == 3:
+                w = state[1] + 1
+                s.append((("walk"), (state[0], w, state[2])))
+            elif state[2] == 4:
+                w = state[0] + 1
+                s.append((("walk"), (w, state[1], state[2])))
+        return s
+        
+Funkcja następnika zwraca stany otrzymane poprzez obrót w prawo i lewo, oraz jeśli to możliwe to ruch do przodu.
 
 ### Definicja przyjętej heurystyki
-
-## Wykorzystane biblioteki
-* heapq
+    def f(self, node):
+        cost = restaurant.tiles[self.goal[1]][self.goal[0]].cost
+        return heuristic((node.state[0], node.state[1]), self.goal) + cost
+       
+   Przyjęta heurystyka zwraca dystans pomiędzy dwoma punktami w linii prostej do którego jest dodany koszt pola, gdzie pole ma koszt 1 dla zwykłego i koszt 5 dla pola z kałużą.