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

106
tractor.py
View File

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