Merge branch 'master' into moving-fixes

tractor.py

@@ -4,89 +4,98 @@ import time
 from queue import PriorityQueue
 from Graph import Graph
 
+class Tractor:
     sleep_time = 1
+    graph = Graph()
+    last_location = None
 
-def heuristic(a, b):
+    def __init__(self, current_location, current_rotation):
+        self.current_location = current_location
+        self.current_rotation = current_rotation
+
+    def heuristic(self, a, b):
         (x1, y1) = a
         (x2, y2) = b
         return abs(x1 - x2) + abs(y1 - y2)
 
-async def rotate(direction):
+    async def rotate(self, direction):
         reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
         writer.write(("rotate " + direction + "\n").encode())
         await reader.readline()
-    time.sleep(sleep_time)
+        time.sleep(self.sleep_time)
         writer.close()
 
-async def try_move():
+    async def try_move(self):
         reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
         writer.write("try\n".encode())
         data = await reader.readline()
         result = data.decode()
-    time.sleep(sleep_time)
+        time.sleep(self.sleep_time)
         writer.close()
         return result
 
-async def move():
+    async def move(self):
         reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
         writer.write("move\n".encode())
         await reader.readline()
-    time.sleep(sleep_time)
+        time.sleep(self.sleep_time)
         writer.close()
 
-async def move_to_neighbor(current, neighbor, direction):
+    async def move_to_neighbor(self, current, neighbor, direction):
         x = current[0] - neighbor[0]
         y = current[1] - neighbor[1]
         if x > 0:
-        final_direction = 'E'
-    elif x < 0:
-        final_direction = 'w'
-    elif y > 0:
-        final_direction = 'S'
-    else:
             final_direction = 'N'
+        elif x < 0:
+            final_direction = 'S'
+        elif y > 0:
+            final_direction = 'W'
+        else:
+            final_direction = 'E'
 
         if final_direction != direction:
-        await rotate(final_direction)
+            await self.rotate(final_direction)
 
-    await move()
+        print("idziemy na " + final_direction)
+
+        await self.move()
         return final_direction
 
-async def move_to_current_location(graph, last_location, current_location, direction):
+    async def move_to_current_location(self, last_location):
-    if last_location == current_location:
+        if last_location == self.current_location:
-        return direction
+            return self.current_rotation
 
-    frontier = PriorityQueue()
+        # frontier = PriorityQueue()
-    frontier.put(last_location, 0)
+        # frontier.put(last_location, 0)
-    came_from = {}
+        # came_from = {}
-    cost_so_far = {}
+        # cost_so_far = {}
-    came_from = []
+        # came_from = []
-    cost_so_far[last_location] = 0
+        # cost_so_far[last_location] = 0
-    final_direction = None
+        # final_direction = self.current_rotation
 
-    while not frontier.empty():
+        # while not frontier.empty():
-        current = frontier.get()
+        #     current = frontier.get()
-        if current == goal:
+        #     if current == goal:
-            break
+        #         break
-        for next in graph.neighbors(current):
+        #     for next in self.graph.neighbors(current):
-            new_cost = cost_so_far[current] + 1
+        #         new_cost = cost_so_far[current] + 1
-            if next not in cost_so_far or new_cost < cost_so_far[next]:
+        #         if next not in cost_so_far or new_cost < cost_so_far[next]:
-                cost_so_far[next] = new_cost
+        #             cost_so_far[next] = new_cost
-                priority = new_cost + heuristic(goal, next)
+        #             priority = new_cost + self.heuristic(goal, next)
-                frontier.put(next, priority)
+        #             frontier.put(next, priority)
-                came_from.append(current)
+        #             came_from.append(current)
 
-    # print(came_from)
+        # for node in came_from[1:]:
-    for node in came_from[1:]:
+        #     final_direction = await self.move_to_neighbor(current, node, final_direction)
-        final_direction = await move_to_neighbor(current, node, final_direction)
+        #     current = node
-        current = node
+        final_direction = await(self.move_to_neighbor(last_location, self.current_location, self.current_rotation))
 
         return final_direction
 
 
-async def move_tractor(start_position, goal):
+    async def move_tractor(self, start_position, goal):
         directions = ['N', 'W', 'S', 'E']
-    current_rotation = 'N'
+        self.current_rotation = 'N'
         last_node = 'N'
         last_location = start_position
 
@@ -96,39 +105,37 @@ async def move_tractor(start_position, goal):
         cost_so_far[start_position] = 0
         start_flag = True
 
-    graph = Graph()
 
         while not frontier.empty():
-        for key in graph.nodes:
+            # for key in self.graph.nodes:
-            print ("key: %s , value: %s" % (key, graph.nodes[key]))
+                # print ("key: %s , value: %s" % (key, self.graph.nodes[key]))
 
-        current = frontier.get()
+            self.current_location = frontier.get()
 
             if start_flag:
                 for direction in directions:
                     if direction == last_node:
                         continue
 
-                await rotate(direction)
+                    await self.rotate(direction)
-                result = await try_move()
+                    result = await self.try_move()
                     if result == "OK\n":
-                    graph.add_neighbor(current, direction)
+                        self.graph.add_neighbor(self.current_location, direction)
 
-        current_rotation = directions[(directions.index(current_rotation) - 1) % 4]
+            self.current_rotation = directions[(directions.index(self.current_rotation) - 1) % 4]
-        last_node = directions[(directions.index(current_rotation) - 1) % 4]
+            last_node = directions[(directions.index(self.current_rotation) - 1) % 4]
 
-        current_rotation = await move_to_current_location(graph, last_location, current, current_rotation)
+            self.current_rotation = await self.move_to_current_location(last_location)
-        print("Jestem na %s , %s" % (current[0], current[1]))
+            last_location = self.current_location
-        last_location = current
 
-        if current == goal:
+            if self.current_location == goal:
                 break
 
-        for next in graph.neighbors(current):
+            for next in self.graph.neighbors(self.current_location):
-            new_cost = cost_so_far[current] + 1
+                new_cost = cost_so_far[self.current_location] + 1
                 if next not in cost_so_far or new_cost < cost_so_far[next]:
                     cost_so_far[next] = new_cost
-                priority = new_cost + heuristic(goal, next)
+                    priority = new_cost + self.heuristic(goal, next)
                     frontier.put(next, priority)
 
             start_flag = False
@@ -136,5 +143,6 @@ async def move_tractor(start_position, goal):
 if __name__ == "__main__":
     start = (0,0)
     goal = (10,6)
-    asyncio.run(move_tractor(start, goal))
+    tractor = Tractor((0,0), 'N')
+    asyncio.run(tractor.move_tractor(start, goal))
     # asyncio.run(test())