s444423/SI_Traktor
3
1
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Zaktualizuj 'pathfinding_tractor.py'

Browse Source
This commit is contained in:
Piotr Rychlicki 2020-05-13 07:17:24 +00:00
parent d57428a5e0
commit 2bd0c27fdd
1 changed files with 81 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
pathfinding_tractor.py
View File

@ -1,82 +1,82 @@
from queue import PriorityQueue from queue import PriorityQueue
import time import time
class pathfinding_tractor(): class pathfinding_tractor():
def __init__(self): def __init__(self):
pass pass
def heuristic(self,a, b): def heuristic(self,a, b):
(x1, y1) = a (x1, y1) = a
(x2, y2) = b (x2, y2) = b
return abs(x1 - x2) + abs(y1 - y2) return abs(x1 - x2) + abs(y1 - y2)
def points(self, point): def points(self, point):
self.point = [] self.point = []
for i in [[point[0],point[1]-1],[point[0]-1,point[1]],[point[0],point[1]+1],[point[0]+1,point[1]]]: for i in [[point[0],point[1]-1],[point[0]-1,point[1]],[point[0],point[1]+1],[point[0]+1,point[1]]]:
if i[0] in [-1,10] or i[1] in [-1,10]: if i[0] in [-1,10] or i[1] in [-1,10]:
pass pass
else: else:
self.point.append(i) self.point.append(i)
return self.point return self.point
def pathfinding_tractor(self,field,traktor,ui,pola_buraczane,i): def pathfinding_tractor(self,field,traktor,ui,pola_buraczane,i):
self.pola = pola_buraczane self.pola = pola_buraczane
self.ui = ui self.ui = ui
self.traktor = traktor self.traktor = traktor
self.start_position = [int(self.pola[i])%10,int(self.pola[i])//10] self.start_position = [int(self.pola[i])%10,int(self.pola[i])//10]
# print(str(self.start_position)) # print(str(self.start_position))
self.field = field self.field = field
self.end_point = [int(self.pola[i+1])%10,int(self.pola[i+1])//10] self.end_point = [int(self.pola[i+1])%10,int(self.pola[i+1])//10]
# print(str(self.end_point)) # print(str(self.end_point))
if self.start_position == self.end_point: if self.start_position == self.end_point:
self.traktor.work() self.traktor.work()
else: else:
self.route = self.a_star(self.start_position,self.end_point) self.route = self.a_star(self.start_position,self.end_point)
for i in self.route[::-1]: for i in self.route[::-1]:
self.poz = self.traktor.get_poz() self.poz = self.traktor.get_poz()
if i[1]> self.poz[1]: if i[1]> self.poz[1]:
self.traktor.move_down() self.traktor.move_down()
elif i[1]< self.poz[1]: elif i[1]< self.poz[1]:
self.traktor.move_up() self.traktor.move_up()
elif i[0]> self.poz[0]: elif i[0]> self.poz[0]:
self.traktor.move_right() self.traktor.move_right()
elif i[0]< self.poz[0]: elif i[0]< self.poz[0]:
self.traktor.move_left() self.traktor.move_left()
self.ui.update() self.ui.update()
time.sleep(0.1) time.sleep(0.1)
self.traktor.work() self.traktor.work()
def a_star(self,start,end): def a_star(self,start,end):
self.a_queue = PriorityQueue() self.a_queue = PriorityQueue()
self.a_queue.put(start,0) self.a_queue.put(start,0)
self.cost = {tuple(start): 0} self.cost = {tuple(start): 0}
self.path_from = {tuple(start): None} self.path_from = {tuple(start): None}
self.finall_path = [tuple(end)] self.finall_path = [tuple(end)]
self.found = 0 self.found = 0
while not self.a_queue.empty(): while not self.a_queue.empty():
self.current = tuple(self.a_queue.get()) self.current = tuple(self.a_queue.get())
if self.current == tuple(end): if self.current == tuple(end):
break break
for self.next in self.points(self.current): for self.next in self.points(self.current):
self.new_cost = self.cost[tuple(self.current)] + self.field.get_value(self.next) self.new_cost = self.cost[tuple(self.current)] + self.field.get_value(self.next)
if tuple(self.next) not in self.cost or self.new_cost < self.cost[tuple(self.next)]: if tuple(self.next) not in self.cost or self.new_cost < self.cost[tuple(self.next)]:
self.cost[tuple(self.next)] = self.new_cost self.cost[tuple(self.next)] = self.new_cost
self.priority = self.new_cost + self.heuristic(end, self.next) self.priority = self.new_cost + self.heuristic(end, self.next)
self.a_queue.put(self.next,self.priority) self.a_queue.put(self.next,self.priority)
self.path_from[tuple(self.next)] = self.current self.path_from[tuple(self.next)] = self.current
if self.next == end: if self.next == end:
self.found = 1 self.found = 1
break break
if self.found: if self.found:
break break
self.pth = self.path_from[tuple(end)] self.pth = self.path_from[tuple(end)]
while not self.pth == tuple(start): while not self.pth == tuple(start):
self.finall_path.append(self.pth) self.finall_path.append(self.pth)
self.pth = self.path_from[self.pth] self.pth = self.path_from[self.pth]
return self.finall_path return self.finall_path