s434713/SZI_PROJEKT_GR1_TRAKTOR
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'master' into moving-fixes

Browse Source
This commit is contained in:
marcinljablonski 2019-05-09 00:51:26 +02:00
commit fbde8d4abc
1 changed files with 127 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
tractor.py
View File

@ -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):
if last_location == current_location:
return direction
async def move_to_current_location(self, last_location):
if last_location == self.current_location:
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 = None
# 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
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)
# 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)
# print(came_from)
for node in came_from[1:]:
final_direction = await move_to_neighbor(current, node, final_direction)
current = node
# 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))
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:
print ("key: %s , value: %s" % (key, graph.nodes[key]))
# for key in self.graph.nodes:
# 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)
result = await try_move()
await self.rotate(direction)
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]
last_node = directions[(directions.index(current_rotation) - 1) % 4]
self.current_rotation = directions[(directions.index(self.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)
print("Jestem na %s , %s" % (current[0], current[1]))
last_location = current
self.current_rotation = await self.move_to_current_location(last_location)
last_location = self.current_location
if current == goal:
if self.current_location == goal:
break
for next in graph.neighbors(current):
new_cost = cost_so_far[current] + 1
for next in self.graph.neighbors(self.current_location):
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())