laby+ray casting

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
+            

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)
-        
+        
+    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

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]))