fixed move to current

tractor.py

@@ -64,38 +64,36 @@ class Tractor:
         await self.move()
         return final_direction
 
+    def dfs_paths(self, start, goal):
+        stack = [(start, [start])]
+        visited = []
+        while stack:
+            (vertex, path) = stack.pop()
+            if vertex not in visited:
+                if vertex == goal:
+                    return path
+                visited.append(vertex)
+                for neighbor in self.graph.nodes[vertex]:
+                    stack.append((neighbor, path + [neighbor]))
+
+
     async def move_to_current_location(self, last_location):
         if last_location == self.current_location:
-            print("olewka")
             return self.current_rotation
 
-        print("nie ma olewka")
-        frontier = PriorityQueue()
-        frontier.put(last_location, 0)
-        cost_so_far = {}
-        came_from = []
-        cost_so_far[last_location] = 0
-        final_direction = self.current_rotation
+        path = self.dfs_paths(last_location, self.current_location)
+        path = path[1:]
+
+        print("~~~~~~~~~~~~~~~~~~")
+        print(path)
         pos = last_location
+        final_direction = self.current_rotation
 
-        while not frontier.empty():
-            current = frontier.get()
-            if current == goal:
-                break
-            for next in self.graph.neighbors(current):
-                new_cost = cost_so_far[current] + 1
-                if next not in cost_so_far or new_cost < cost_so_far[next]:
-                    cost_so_far[next] = new_cost
-                    priority = new_cost + self.heuristic(goal, next)
-                    frontier.put(next, priority)
-                    came_from.append(current)
-
-        came_from = reversed(came_from[::len(came_from)-1])
-        for node in came_from:
+        for node in path:
             final_direction = await self.move_to_neighbor(pos, node, final_direction)
             pos = node
 
-        # final_direction = await(self.move_to_neighbor(last_location, self.current_location, self.current_rotation))
+        # # final_direction = await(self.move_to_neighbor(last_location, self.current_location, self.current_rotation))
         self.moved_flag = True
 
         return final_direction
@@ -135,8 +133,8 @@ class Tractor:
             self.last_node = self.directions[(self.directions.index(self.current_rotation) - 1) % 4]
 
             self.current_rotation = await self.move_to_current_location(last_location)
-            print(last_location)
-            print(self.current_location)
+            # print(last_location)
+            # print(self.current_location)
             last_location = self.current_location
 
             if self.current_location == goal: