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

.

Browse Source
This commit is contained in:
marcinljablonski 2019-05-09 01:37:11 +02:00
parent fbde8d4abc
commit 265137d29b
1 changed files with 47 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
tractor.py
View File

@ -8,10 +8,13 @@ class Tractor:
sleep_time = 1
graph = Graph()
last_location = None
moved_flag = False
directions = ['N', 'W', 'S', 'E']
def __init__(self, current_location, current_rotation):
self.current_location = current_location
self.current_rotation = current_rotation
self.last_node = current_rotation
def heuristic(self, a, b):
(x1, y1) = a
@ -63,48 +66,61 @@ class Tractor:
async def move_to_current_location(self, last_location):
if last_location == self.current_location:
print("olewka")
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 = self.current_rotation
print("nie ma olewka")
frontier = PriorityQueue()
frontier.put(last_location, 0)
cost_so_far = {}
came_from = []
cost_so_far[last_location] = 0
final_direction = self.current_rotation
pos = last_location
# 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)
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)
# 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))
came_from = reversed(came_from[::len(came_from)-1])
for node in came_from:
final_direction = await self.move_to_neighbor(pos, node, final_direction)
pos = node
# final_direction = await(self.move_to_neighbor(last_location, self.current_location, self.current_rotation))
self.moved_flag = True
return final_direction
async def look_around(self):
for direction in self.directions:
if direction == self.last_node:
continue
await self.rotate(direction)
result = await self.try_move()
if result == "OK\n":
self.graph.add_neighbor(self.current_location, direction)
async def move_tractor(self, start_position, goal):
directions = ['N', 'W', 'S', 'E']
self.current_rotation = 'N'
last_node = 'N'
last_location = start_position
frontier = PriorityQueue()
frontier.put(start_position, 0)
cost_so_far = {}
cost_so_far[start_position] = 0
start_flag = True
await self.look_around()
while not frontier.empty():
# for key in self.graph.nodes:
@ -112,20 +128,15 @@ class Tractor:
self.current_location = frontier.get()
if start_flag:
for direction in directions:
if direction == last_node:
continue
if self.moved_flag:
await self.look_around()
await self.rotate(direction)
result = await self.try_move()
if result == "OK\n":
self.graph.add_neighbor(self.current_location, direction)
self.current_rotation = directions[(directions.index(self.current_rotation) - 1) % 4]
last_node = directions[(directions.index(self.current_rotation) - 1) % 4]
self.current_rotation = self.directions[(self.directions.index(self.current_rotation) - 1) % 4]
self.last_node = self.directions[(self.directions.index(self.current_rotation) - 1) % 4]
self.current_rotation = await self.move_to_current_location(last_location)
print(last_location)
print(self.current_location)
last_location = self.current_location
if self.current_location == goal:
@ -138,7 +149,6 @@ class Tractor:
priority = new_cost + self.heuristic(goal, next)
frontier.put(next, priority)
start_flag = False
if __name__ == "__main__":
start = (0,0)