diff --git a/laby.py b/laby.py new file mode 100644 index 0000000..0074c38 --- /dev/null +++ b/laby.py @@ -0,0 +1,76 @@ +import pygame + +from math import dist + +class Laby: + L1=1.50 + L2=2.00 + L3=0.20 + L4=0.40 + L5=0.20 + L6=0.40 + L7=L4+L2-L5-L6 + L8=0.40 + L9=0.40 + L10=0.40 + L11=1.10 + L12=2.95 + L13=0 + L14=0.40 + L15=0.90 + L16=0.20 + L17=0.20 + walls = [ + ((0,0),(0,L17)), + ((0,L17+L4),(0,L17+L4+L2)), + ((0,0),(L8,0)), + ((L8,0),(L8,L7)), + ((L8,L7),(L8+L3,L7)), + ((L8+L3,L7),(L8+L3,0)), + ((L8+L3,0),(L8+L3+L9,0)), + ((L8+L3+L9,0),(L8+L3+L9,L17)), + + ((0,L17+L4),(0,L17+L4+L2)), + ((0,L17+L4+L2),(L8,L17+L4+L2)), + ((L8,L17+L4+L2),(L8,L17+L4+L2-L5)), + ((L8,L17+L4+L2-L5),(L8+L3,L17+L4+L2-L5)), + ((L8+L3,L17+L4+L2-L5),(L8+L3,L17+L4+L2)), + ((L8+L3,L17+L4+L2),(L8+L3+L9,L17+L4+L2)), + ((L8+L3+L9,L17+L4+L2),(L8+L3+L9,L17+L10)), + ] + + def __init__(self, pos): + self.pos = pos + self.walls = [([w+p for w,p in zip(l[0], self.pos)],[w+p for w,p in zip(l[1], self.pos)]) for l in self.walls] + + def draw(self, surf): + for l in self.walls: + pygame.draw.line(surf, (255,255,255), [100*wp for wp in l[0]], [100*wp for wp in l[1]]) + + def get_closest_wall_dist(self, ray): + inters = [self.get_inter(ray,w) for w in self.walls] + inters = list(filter(None, inters)) + if len(inters) == 0: + return None + dists = [dist(ray[0], inter) for inter in inters] + index_min = min(range(len(dists)), key=dists.__getitem__) + closest = inters[index_min] + return closest + + def get_inter(self,l1,l2): + (x1,y1),(x2,y2) = l1 + (x3,y3),(x4,y4) = l2 + # https://en.wikipedia.org/wiki/Line–line_intersection#Given_two_points_on_each_line_segment + tn = (x1-x3)*(y3-y4) - (y1-y3)*(x3-x4) + td = (x1-x2)*(y3-y4) - (y1-y2)*(x3-x4) + un = (x1-x3)*(y1-y2) - (y1-y3)*(x1-x2) + ud = (x1-x2)*(y3-y4) - (y1-y2)*(x3-x4) + if td == 0 or ud == 0: + return None + t = tn/td + u = un/ud + if not (0 <= t <= 1 and 0 <= u <= 1): + return None + pint = (x1+t*(x2-x1),y1+t*(y2-y1)) + return pint + \ No newline at end of file diff --git a/robot.py b/robot.py index 4197a61..6316c6e 100644 --- a/robot.py +++ b/robot.py @@ -1,8 +1,8 @@ -from math import cos, sin, pi, atan2, tan +from math import cos, sin, pi, atan2, tan, dist class Robot: # position - pos=(1,1.5) + pos=(0.1,1) # orientation rot=-pi/2 # velocite @@ -12,7 +12,7 @@ class Robot: # velocite des roues (non angulaire) wv=[0,0] # velocite max - wmv=1 + wmv=0.1 # espace entre les deux roues l=0.1 # time @@ -35,8 +35,15 @@ class Robot: PHOTORES_BRIDGE = 1000 pr_l_pin = 0 # A0 voir adapter_ard.h pr_r_pin = 1 # A1 - - def __init__(self): + # rayon de mesure des capteurs ultrason + ultr_sense_max = 4 + ultr_sense_rays = (pi/2, -pi/2) + dists = [None] * len(ultr_sense_rays) + tr = [0]*len(ultr_sense_rays) + ci = [0]*len(tr) + + def __init__(self, laby): + self.laby = laby self.pr_max_ang = tan((self.photores_l/2)/self.photores_sep_l) self.t = 0 self.dt = -1 @@ -45,6 +52,7 @@ class Robot: def update(self): if(self.dt == -1): return + self.update_ultr_sense() self.compute_light() print("***********", self.t, "****", self.dt) # https://robotics.stackexchange.com/questions/106/what-is-a-suitable-model-for-two-wheeled-robots @@ -90,4 +98,13 @@ class Robot: self.dt = (t/1000000 - self.t) self.t = t/1000000 assert(self.dt < 0.1) - \ No newline at end of file + + def update_ultr_sense(self): + for ind,r in enumerate(self.ultr_sense_rays): + tr = (self.pos[0]+self.ultr_sense_max*sin(r+self.rot), self.pos[1]+self.ultr_sense_max*cos(r+self.rot)) + self.tr[ind] = tr + self.ci[ind] = self.laby.get_closest_wall_dist((self.pos, tr)) + if self.ci[ind]: + self.dists[ind] = dist(self.pos, self.ci[ind]) + else: + self.dists[ind] = None diff --git a/simulateur.py b/simulateur.py index 02bfc86..bc43d01 100644 --- a/simulateur.py +++ b/simulateur.py @@ -1,5 +1,6 @@ from robot import Robot from parser import aip_parser +from laby import Laby from subprocess import Popen, PIPE import fcntl @@ -12,14 +13,11 @@ import sys, pygame from pygame.locals import * pygame.init() -size = width, height = 1000, 300 +size = width, height = 500, 300 black = 0, 0, 0 screen = pygame.display.set_mode(size) -# le robot -r = Robot() - # on lance le programme arduino ard_process = Popen(["build/app"], stdin=PIPE, stdout=PIPE); @@ -28,9 +26,17 @@ fd = ard_process.stdout.fileno() fl = fcntl.fcntl(fd, fcntl.F_GETFL) fcntl.fcntl(fd, fcntl.F_SETFL, fl | os.O_NONBLOCK) +laby = Laby((1.5,0)) + +# le robot +r = Robot(laby) +r.light_pos = (laby.pos[0]+laby.L8+laby.L3/2,laby.L17+laby.L4/2) +r.pos = (0.1, laby.L17+laby.L4/2) + # le parser wrapper arduino->simulateur parser = aip_parser(ard_process.stdin, ard_process.stdout, r); + while True: for event in pygame.event.get(): if event.type == pygame.QUIT: sys.exit() @@ -54,8 +60,10 @@ while True: # On dessine le bordel screen.fill(black) + laby.draw(screen) robotShape = pygame.Rect(20,15, 10,20); rsurf = pygame.Surface((50,50)).convert_alpha(); + rsurf.fill((255,255,255,0)) # robot pygame.draw.rect(rsurf, (255,255,255), robotShape); # heading @@ -63,7 +71,13 @@ while True: # dir2light pygame.draw.line(rsurf, (255,255,255), (25,25), (25+25*sin(r.angle2spot),25+25*cos(r.angle2spot))) # light - pygame.draw.polygon(screen, (255,255,0), ([100*a for a in r.light_pos],[100*(a+b) for a,b in zip(r.light_pos,(-1,1))],[100*(a+b) for a,b in zip(r.light_pos,(-1,-1))])) + pygame.draw.polygon(screen, (255,255,0), ([100*a for a in r.light_pos],[100*(a+b) for a,b in zip(r.light_pos,(-0.1,0.1))],[100*(a+b) for a,b in zip(r.light_pos,(-0.1,-0.1))])) + # ultrasonic sensors + for usrs in r.tr: + pygame.draw.line(screen, (255,0,0), [p*100 for p in r.pos], [p*100 for p in usrs]) + for c in r.ci: + if c: + pygame.draw.circle(screen, (255,0,0), [p*100 for p in c], 3) rsurf_r = pygame.transform.rotate(rsurf, degrees(r.rot)); center = rsurf_r.get_rect().center screen.blit(rsurf_r, (r.pos[0]*100-center[0],r.pos[1]*100-center[1]))