112 lines
4.1 KiB
Plaintext
112 lines
4.1 KiB
Plaintext
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)
|
|
|
|
|
|
async def tractor():
|
|
directions = ['N', 'W', 'S', 'E']
|
|
current_rotation = 'S'
|
|
start = (0,0)
|
|
current_position = start
|
|
goal = (10,6)
|
|
|
|
frontier = PriorityQueue()
|
|
frontier.put(start, 0)
|
|
came_from = {}
|
|
cost_so_far = {}
|
|
came_from[start] = None
|
|
cost_so_far[start] = 0
|
|
start_flag = True
|
|
|
|
while not frontier.empty():
|
|
local_graph = []
|
|
is_already_moved = False
|
|
for i, direction in enumerate(directions):
|
|
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
|
writer.write("move\n".encode())
|
|
data = await reader.readline()
|
|
result = data.decode()
|
|
writer.close()
|
|
time.sleep(0.7)
|
|
if result == "OK":
|
|
print("OK")
|
|
if direction == 'N':
|
|
local_graph.append((current_position[0] - 1, current_position[1]))
|
|
elif direction == 'S':
|
|
local_graph.append((current_position[0] + 1, current_position[1]))
|
|
elif direction == 'W':
|
|
local_graph.append((current_position[0], current_position[1] - 1 ))
|
|
else:
|
|
local_graph.append((current_position[0], current_position[1] + 1 ))
|
|
|
|
if i == 3 and len(local_graph) == 1:
|
|
is_already_moved = True
|
|
else:
|
|
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
|
writer.write(("rotate " + directions[((i + 2) % 4)] + "\n").encode())
|
|
data = await reader.readline()
|
|
writer.close()
|
|
time.sleep(0.7)
|
|
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
|
writer.write("move\n".encode())
|
|
data = await reader.readline()
|
|
writer.close()
|
|
time.sleep(0.7)
|
|
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
|
current_rotation = directions[((i - 1) % 4)]
|
|
writer.write(("rotate " + current_rotation + "\n").encode())
|
|
data = await reader.readline()
|
|
writer.close()
|
|
time.sleep(0.7)
|
|
|
|
current = frontier.get()
|
|
if not start_flag and not is_already_moved:
|
|
if current[0] == current_position[0] and current[1] > current_position[1]:
|
|
current_rotation = 'E'
|
|
elif current[0] == current_position[0] and current[1] < current_position[1]:
|
|
current_rotation = 'W'
|
|
elif current[0] > current_position[0]:
|
|
current_rotation = 'S'
|
|
elif current[0] < current_position[0]:
|
|
current_rotation = 'N'
|
|
|
|
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
|
writer.write(("rotate " + current_rotation + "\n").encode())
|
|
data = await reader.readline()
|
|
writer.close()
|
|
time.sleep(0.7)
|
|
reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
|
|
writer.write("move\n".encode())
|
|
data = await reader.readline()
|
|
writer.close()
|
|
time.sleep(0.7)
|
|
|
|
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)
|
|
came_from[next] = current
|
|
|
|
start_flag = False
|
|
# reader, writer = await asyncio.open_connection(
|
|
# '127.0.0.1', 8888)
|
|
|
|
# time.sleep(0.7)
|
|
|
|
# writer.write("rotate E\n".encode())
|
|
# data = await reader.readline()
|
|
# sys.stdout.write(data.decode())
|
|
# time.sleep(0.7)
|
|
asyncio.run(tractor())
|