111 lines
3.1 KiB
Python
111 lines
3.1 KiB
Python
import asyncio
|
|
import sys
|
|
import time
|
|
from queue import PriorityQueue
|
|
|
|
def heuristic(a, b):
|
|
(x1, y1) = a
|
|
(x2, y2) = b
|
|
return abs(x1 - x2) + abs(y1 - y2)
|
|
|
|
def addnode(local_graph, direction):
|
|
if direction == 'N':
|
|
local_graph.append((current[0] - 1, current[1]))
|
|
elif direction == 'S':
|
|
local_graph.append((current[0] + 1, current[1]))
|
|
elif direction == 'W':
|
|
local_graph.append((current[0], current[1] - 1 ))
|
|
else:
|
|
local_graph.append((current[0], current[1] + 1 ))
|
|
|
|
return local_graph
|
|
|
|
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 move_tractor(start_position, goal):
|
|
directions = ['N', 'W', 'S', 'E']
|
|
current_rotation = 'N'
|
|
last_node = None
|
|
current_position = start_position
|
|
|
|
frontier = PriorityQueue()
|
|
frontier.put(start, 0)
|
|
cost_so_far = {}
|
|
cost_so_far[start] = 0
|
|
start_flag = True
|
|
|
|
while not frontier.empty():
|
|
local_graph = []
|
|
|
|
for direction in directions:
|
|
if direction == last_node:
|
|
continue
|
|
|
|
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
|
writer.write(("rotate " + direction + "\n").encode())
|
|
await reader.readline()
|
|
time.sleep(0.7)
|
|
writer.close()
|
|
|
|
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(0.7)
|
|
writer.close()
|
|
|
|
if result == "OK":
|
|
local_graph = addnode(local_graph, direction)
|
|
|
|
current_rotation = directions[(directions.index(current_rotation) - 1) % 4]
|
|
|
|
|
|
current = frontier.get()
|
|
if not start_flag:
|
|
rotation = getrotation(current_position, current)
|
|
if rotation != current_rotation:
|
|
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
|
writer.write(("rotate " + rotation + "\n").encode())
|
|
await reader.readline()
|
|
current_rotation = rotation
|
|
time.sleep(0.7)
|
|
writer.close()
|
|
last_node = directions[(directions.index(rotation) - 2) % 4]
|
|
|
|
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
|
writer.write("move\n".encode())
|
|
await reader.readline()
|
|
current_position = current
|
|
time.sleep(0.7)
|
|
writer.close()
|
|
|
|
if current == goal:
|
|
break
|
|
|
|
for next in local_graph:
|
|
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)
|
|
print(next)
|
|
|
|
start_flag = False
|
|
writer.close()
|
|
|
|
if __name__ == "__main__":
|
|
start = (0,0)
|
|
goal = (10,6)
|
|
asyncio.run(move_tractor(start, goal))
|