.

tractor.py

@@ -8,10 +8,13 @@ class Tractor:
     sleep_time = 1
     graph = Graph()
     last_location = None
+    moved_flag = False
+    directions = ['N', 'W', 'S', 'E']
 
     def __init__(self, current_location, current_rotation):
         self.current_location = current_location
         self.current_rotation = current_rotation
+        self.last_node = current_rotation
 
     def heuristic(self, a, b):
         (x1, y1) = a
@@ -63,48 +66,61 @@ class Tractor:
 
     async def move_to_current_location(self, last_location):
         if last_location == self.current_location:
+            print("olewka")
             return self.current_rotation
 
-        # frontier = PriorityQueue()
-        # frontier.put(last_location, 0)
-        # came_from = {}
-        # cost_so_far = {}
-        # came_from = []
-        # cost_so_far[last_location] = 0
-        # final_direction = 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
+        pos = last_location
 
-        # 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)
+        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)
 
-        # for node in came_from[1:]:
-        #     final_direction = await self.move_to_neighbor(current, node, final_direction)
-        #     current = node
-        final_direction = await(self.move_to_neighbor(last_location, self.current_location, self.current_rotation))
+        came_from = reversed(came_from[::len(came_from)-1])
+        for node in came_from:
+            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))
+        self.moved_flag = True
 
         return final_direction
 
+    async def look_around(self):
+        for direction in self.directions:
+            if direction == self.last_node:
+                continue
+
+            await self.rotate(direction)
+            result = await self.try_move()
+            if result == "OK\n":
+                self.graph.add_neighbor(self.current_location, direction)
+
 
     async def move_tractor(self, start_position, goal):
-        directions = ['N', 'W', 'S', 'E']
         self.current_rotation = 'N'
-        last_node = 'N'
         last_location = start_position
 
         frontier = PriorityQueue()
         frontier.put(start_position, 0)
         cost_so_far = {}
         cost_so_far[start_position] = 0
-        start_flag = True
 
+        await self.look_around()
 
         while not frontier.empty():
             # for key in self.graph.nodes:
@@ -112,20 +128,15 @@ class Tractor:
 
             self.current_location = frontier.get()
 
-            if start_flag:
-                for direction in directions:
-                    if direction == last_node:
-                        continue
+            if self.moved_flag:
+                await self.look_around()
 
-                    await self.rotate(direction)
-                    result = await self.try_move()
-                    if result == "OK\n":
-                        self.graph.add_neighbor(self.current_location, direction)
-
-            self.current_rotation = directions[(directions.index(self.current_rotation) - 1) % 4]
-            last_node = directions[(directions.index(self.current_rotation) - 1) % 4]
+            self.current_rotation = self.directions[(self.directions.index(self.current_rotation) - 1) % 4]
+            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)
             last_location = self.current_location
 
             if self.current_location == goal:
@@ -138,7 +149,6 @@ class Tractor:
                     priority = new_cost + self.heuristic(goal, next)
                     frontier.put(next, priority)
 
-            start_flag = False
 
 if __name__ == "__main__":
     start = (0,0)