.

Graph.py

@@ -1,7 +1,28 @@
-class SimpleGraph:
+class Graph:
     def __init__(self):
-        self.edges = {}
+        self.nodes = {}
     
     def neighbors(self, id):
-        return self.edges[id]
+        return self.nodes[id]
+
+    def direction2point(self, point, direction):
+        if direction == 'N':
+            return (point[0] - 1, point[1])
+        elif direction == 'S':
+            return (point[0] + 1, point[1])
+        elif direction == 'W':
+            return (point[0], point[1] - 1)
+        else:
+            return (point[0], point[1] + 1)
+
+    def add_neighbor(self, node, direction):
+        neighbor = self.direction2point(node, direction)
+        if node in self.nodes:
+            self.nodes[node].append(neighbor)
+        else:
+            self.nodes[node] = [neighbor]
+        if neighbor in self.nodes:
+            self.nodes[neighbor].append(node)
+        else:
+            self.nodes[neighbor] = [node]
 

Point.py

@@ -1,17 +0,0 @@
-class Point:
-    def __init__(self, cord):
-        self.x = cord[0]
-        self.y = cord[1]
-
-    def get_cord(self):
-        return (self.x, self.y)
-
-    def get_x(self):
-        return self.x
-
-    def get_y(self):
-        return self.y
-
-    def set_cord(self, cord):
-        self.x = cord[0]
-        self.y = cord[1]

env.py

@@ -37,6 +37,7 @@ def print_field(field, tractor):
         os.system("cls")
     else:
         os.system("clear")
+    print(tractor.get_position())
     sys.stdout.write(OKBLUE)
     sys.stdout.write("$")
     sys.stdout.write(" ")

tractor.py

@@ -2,40 +2,20 @@ import asyncio
 import sys
 import time
 from queue import PriorityQueue
+from Graph import Graph
+
+sleep_time = 1
 
 def heuristic(a, b):
     (x1, y1) = a
     (x2, y2) = b
     return abs(x1 - x2) + abs(y1 - y2)
 
-def addnode(current, direction):
-    if direction == 'N':
-        return (current[0] - 1, current[1])
-    elif direction == 'S':
-        return (current[0] + 1, current[1])
-    elif direction == 'W':
-        return (current[0], current[1] - 1)
-    else:
-        return (current[0], current[1] + 1)
-
-
-def getrotation(a, b):
-    y = b[0] - a[0]
-    x = b[1] - a[1]
-    if y > 0:
-        return 'S'
-    elif y < 0:
-        return 'N'
-    elif x > 0:
-        return 'E'
-    else:
-        return 'W'
-
 async def rotate(direction):
     reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
     writer.write(("rotate " + direction + "\n").encode())
     await reader.readline()
-    time.sleep(0.7)
+    time.sleep(sleep_time)
     writer.close()
 
 async def try_move():
@@ -43,7 +23,7 @@ async def try_move():
     writer.write("try\n".encode())
     data = await reader.readline()
     result = data.decode()
-    time.sleep(0.7)
+    time.sleep(sleep_time)
     writer.close()
     return result
 
@@ -51,52 +31,100 @@ async def move():
     reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
     writer.write("move\n".encode())
     await reader.readline()
-    time.sleep(0.7)
+    time.sleep(sleep_time)
     writer.close()
 
+async def move_to_neighbor(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'
+
+    if final_direction != direction:
+        await rotate(final_direction)
+
+    await move()
+    return final_direction
+
+async def move_to_current_location(graph, last_location, current_location, direction):
+    if last_location == current_location:
+        return direction
+
+    frontier = PriorityQueue()
+    frontier.put(last_location, 0)
+    came_from = {}
+    cost_so_far = {}
+    came_from = []
+    cost_so_far[last_location] = 0
+    final_direction = None
+
+    while not frontier.empty():
+        current = frontier.get()
+        if current == goal:
+            break
+        for next in 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 + heuristic(goal, next)
+                frontier.put(next, priority)
+                came_from.append(current)
+
+    # print(came_from)
+    for node in came_from[1:]:
+        final_direction = await move_to_neighbor(current, node, final_direction)
+        current = node
+
+    return final_direction
+
 
 async def move_tractor(start_position, goal):
     directions = ['N', 'W', 'S', 'E']
     current_rotation = 'N'
-    last_node = None
-    current_position = start_position
+    last_node = 'N'
+    last_location = start_position
 
     frontier = PriorityQueue()
-    frontier.put(start, 0)
+    frontier.put(start_position, 0)
     cost_so_far = {}
-    cost_so_far[start] = 0
+    cost_so_far[start_position] = 0
     start_flag = True
 
+    graph = Graph()
+
     while not frontier.empty():
-        local_graph = []
-
-        for direction in directions:
-            if direction == last_node:
-                continue
-
-            await rotate(direction)
-
-            result = await try_move()
-
-            if result == "OK\n":
-                local_graph.append(addnode(current_position, direction))
-
-        current_rotation = directions[(directions.index(current_rotation) - 1) % 4]
-
+        for key in graph.nodes:
+            print ("key: %s , value: %s" % (key, graph.nodes[key]))
 
         current = frontier.get()
-        if not start_flag:
-            rotation = getrotation(current_position, current)
-            if rotation != current_rotation:
-                await rotate(rotation)
-            last_node = directions[(directions.index(rotation) - 2) % 4]
-            await move()
-        current_position = current
+
+        if start_flag:
+            for direction in directions:
+                if direction == last_node:
+                    continue
+
+                await rotate(direction)
+                result = await try_move()
+                if result == "OK\n":
+                    graph.add_neighbor(current, direction)
+
+        current_rotation = directions[(directions.index(current_rotation) - 1) % 4]
+        last_node = directions[(directions.index(current_rotation) - 1) % 4]
+
+        current_rotation = await move_to_current_location(graph, last_location, current, current_rotation)
+        print("Jestem na %s , %s" % (current[0], current[1]))
+        last_location = current
 
         if current == goal:
             break
 
-        for next in local_graph:
+        for next in 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
@@ -109,3 +137,4 @@ if __name__ == "__main__":
     start = (0,0)
     goal = (10,6)
     asyncio.run(move_tractor(start, goal))
+    # asyncio.run(test())