X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fdisplay.py;h=edcf494603b69d73d9e407fd39d227c331b23a9d;hp=239b2d6a9f0aebd901e4e5a3a0a8946d5f108da2;hb=aed049ab6be4e1916457743d53f6b610a21b4854;hpb=ebfaaedd491e61696cc93b353471be15408d23e4

diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index 239b2d6..edcf494 100644
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -1,9 +1,15 @@
-import math, sys, time, os, random
+import math, sys, time, os, random, re
 from visual import *
 
+FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
+INT = "([-+]?[0-9][0-9]*)"
+
 AREA_X = 3000.
 AREA_Y = 2100.
 
+ROBOT_HEIGHT=5 # 350
+CORN_HEIGHT=5  # 150
+
 area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
 areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
 area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
@@ -13,16 +19,28 @@ scene.autoscale=0
 # all positions of robot every 5ms
 save_pos = []
 
-robot = box(pos = (0, 0, 150),
-            size = (250,320,350),
-            color = (1, 0, 0) )
+robot = box(color=(0.4, 0.4, 0.4))
+lspickle = box(color=(0.4, 0.4, 0.4))
+rspickle = box(color=(0.4, 0.4, 0.4))
 
-last_pos = robot.pos.x, robot.pos.y, robot.pos.z
+last_pos = (0.,0.,0.)
 hcenter_line = curve()
 hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
 vcenter_line = curve()
 vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
 
+yellowarea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
+yellowareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , yellowarea)
+yellowarea_box = box(pos=(-AREA_X/2+250,-AREA_Y/2+250,0), size=yellowareasize, color=(1.0, 1.0, 0.0))
+
+bluearea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
+blueareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , bluearea)
+bluearea_box = box(pos=(AREA_X/2-250,-AREA_Y/2+250,0), size=blueareasize, color=(0.0, 0.0, 1.0))
+
+greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ]
+greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea)
+greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3))
+
 def square(sz):
     sq = curve()
     sq.pos = [(-sz, -sz, 0.3),
@@ -35,6 +53,11 @@ def square(sz):
 sq1 = square(250)
 sq2 = square(500)
 
+robot_x = 0.
+robot_y = 0.
+robot_a = 0.
+robot_lspickle = 0
+robot_rspickle = 0
 robot_trail = curve()
 robot_trail_list = []
 max_trail = 500
@@ -54,7 +77,8 @@ TYPE_DANGEROUS=1
 TYPE_WHITE_CORN=2
 TYPE_BLACK_CORN=3
 TYPE_OBSTACLE=4
-TYPE_NEIGH=5
+TYPE_BALL=5
+TYPE_NEIGH=6
 
 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
@@ -131,6 +155,15 @@ def init_waypoints():
             if c >= 0:
                 waypoints[i][j] = corn_table[c]
                 continue
+
+            # balls
+            if (i & 1) == 0 and j > 3:
+                waypoints[i][j] = TYPE_BALL
+                continue
+            if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2:
+                waypoints[i][j] = TYPE_BALL
+                continue
+
             # too close of border
             if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
                 waypoints[i][j] = TYPE_OBSTACLE
@@ -156,6 +189,21 @@ def init_waypoints():
 def toggle_obj_disp():
     global area_objects
 
+    """
+    if area_objects == []:
+        c = sphere(radius=5, color=(0., 0.,1.),
+                   pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5))
+        area_objects.append(c)
+        c = sphere(radius=5, color=(0., 0.,1.),
+                   pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5))
+        area_objects.append(c)
+        c = sphere(radius=5, color=(0., 0.,1.),
+                   pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5))
+        area_objects.append(c)
+        c = sphere(radius=5, color=(0., 0.,1.),
+                   pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5))
+        area_objects.append(c)
+"""
     if area_objects == []:
         i = 0
         j = 0
@@ -169,14 +217,22 @@ def toggle_obj_disp():
             while y < 2100:
                 print x,y
                 if waypoints[i][j] == TYPE_WHITE_CORN:
-                    c = cylinder(axis=(0,0,1), length=150,
+                    c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
                                  radius=25, color=(0.8,0.8,0.8),
-                                 pos=(x-AREA_X/2,y-AREA_Y/2,75))
+                                 pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
                     area_objects.append(c)
                 elif waypoints[i][j] == TYPE_BLACK_CORN:
-                    c = cylinder(axis=(0,0,1), length=150,
+                    c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
                                  radius=25, color=(0.2,0.2,0.2),
-                                 pos=(x-AREA_X/2,y-AREA_Y/2,75))
+                                 pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
+                    area_objects.append(c)
+                elif waypoints[i][j] == TYPE_BALL:
+                    c = sphere(radius=50, color=(1., 0.,0.),
+                               pos=(x-AREA_X/2,y-AREA_Y/2,50))
+                    area_objects.append(c)
+                else:
+                    c = sphere(radius=5, color=(0., 0.,1.),
+                               pos=(x-AREA_X/2,y-AREA_Y/2,5))
                     area_objects.append(c)
                 j += 1
                 y += STEP_CORN_Y
@@ -190,18 +246,32 @@ def toggle_obj_disp():
                 o.visible = 1
 
 
-def set_robot(x, y, a):
+def set_robot():
     global robot, last_pos, robot_trail, robot_trail_list
-    global save_pos
+    global save_pos, robot_x, robot_y, robot_a
 
-    robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150)
-    robot.axis = (math.cos(a*math.pi/180),
-                  math.sin(a*math.pi/180),
-                  0)
-    robot.size = (250, 320, 350)
+    axis = (math.cos(robot_a*math.pi/180),
+            math.sin(robot_a*math.pi/180),
+            0)
+
+    robot.pos = (robot_x - AREA_X/2, robot_y - AREA_Y/2, ROBOT_HEIGHT/2)
+    robot.axis = axis
+    robot.size = (250, 320, ROBOT_HEIGHT)
+
+    lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a-90)*math.pi/180),
+                    robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a-90)*math.pi/180),
+                    ROBOT_HEIGHT/2)
+    lspickle.axis = axis
+    lspickle.size = (20, 320, 5)
+
+    rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a+90)*math.pi/180),
+                    robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a+90)*math.pi/180),
+                    ROBOT_HEIGHT/2)
+    rspickle.axis = axis
+    rspickle.size = (20, 320, 5)
 
     # save position
-    save_pos.append((robot.pos.x, robot.pos, a))
+    save_pos.append((robot.pos.x, robot.pos, robot_a))
 
     pos = robot.pos.x, robot.pos.y, 0.3
     if pos != last_pos:
@@ -238,12 +308,41 @@ while True:
         fr = open("/tmp/.robot_sim2dis", "r")
         fw = open("/tmp/.robot_dis2sim", "w", 0)
         while True:
+            m = None
             l = fr.readline()
-            try:
-                x,y,a = map(lambda x:int(x), l[:-1].split(" "))
-                set_robot(x,y,a)
-            except ValueError:
-                pass
+
+            # parse position
+            if not m:
+                m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l)
+                if m:
+                    robot_x = int(m.groups()[0])
+                    robot_y = int(m.groups()[1])
+                    robot_a = int(m.groups()[2])
+                    set_robot()
+
+            # parse ballboard
+            if not m:
+                m = re.match("ballboard=%s"%(INT), l)
+                if m:
+                    print int(m.groups()[0])
+
+            # parse cobboard
+            if not m:
+                m = re.match("cobboard=%s"%(INT), l)
+                if m:
+                    mode = int(m.groups()[0])
+                    if (mode & 0x01) == 0:
+                        robot_lspickle = 0
+                    elif (mode & 0x02) == 0:
+                        robot_lspickle = 1
+                    else:
+                        robot_lspickle = 2
+                    if (mode & 0x04) == 0:
+                        robot_rspickle = 0
+                    elif (mode & 0x08) == 0:
+                        robot_rspickle = 1
+                    else:
+                        robot_rspickle = 2
 
             if scene.kb.keys == 0:
                 continue