X-Git-Url: http://git.droids-corp.org/?a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fdisplay.py;h=f89580cad18aed5395e54893341e199098e96ba7;hb=dab083b5d263fdf9675362bfe5a85f993a43a6c6;hp=64fa521ca5c024ec416a5afa0facd60b8807ce10;hpb=6914527de2ecfef9d790740c71739e7418246b96;p=aversive.git

diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index 64fa521..f89580c 100644
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -10,6 +10,9 @@ AREA_Y = 2100.
 ROBOT_HEIGHT=5 # 350
 CORN_HEIGHT=5  # 150
 
+ROBOT_WIDTH=320
+ROBOT_LENGTH=360
+
 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))
@@ -41,6 +44,10 @@ 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))
 
+YELLOW = 0
+BLUE = 1
+color = YELLOW
+
 def square(sz):
     sq = curve()
     sq.pos = [(-sz, -sz, 0.3),
@@ -245,33 +252,51 @@ def toggle_obj_disp():
             else:
                 o.visible = 1
 
+def toggle_color():
+    global color
+    global BLUE, YELLOW
+    if color == YELLOW:
+        color = BLUE
+    else:
+        color = YELLOW
 
 def set_robot():
     global robot, last_pos, robot_trail, robot_trail_list
     global save_pos, robot_x, robot_y, robot_a
 
-    axis = (math.cos(robot_a*math.pi/180),
-            math.sin(robot_a*math.pi/180),
+    if color == YELLOW:
+        tmp_x = robot_x - AREA_X/2
+        tmp_y = robot_y - AREA_Y/2
+        tmp_a = robot_a
+    else:
+        tmp_x = -robot_x + AREA_X/2
+        tmp_y = -robot_y + AREA_Y/2
+        tmp_a = robot_a
+
+    robot.pos = (tmp_x, tmp_y, ROBOT_HEIGHT/2)
+    axis = (math.cos(tmp_a*math.pi/180),
+            math.sin(tmp_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)
+    robot.size = (ROBOT_LENGTH, ROBOT_WIDTH, ROBOT_HEIGHT)
 
-    lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*70) * math.cos((robot_a-90)*math.pi/180),
-                    robot_y - AREA_Y/2 + (robot_lspickle*70) * math.sin((robot_a-90)*math.pi/180),
+    robot_lspickle = 2 # XXX
+    lspickle.pos = (tmp_x + (robot_lspickle*60) * math.cos((tmp_a+90)*math.pi/180),
+                    tmp_y + (robot_lspickle*60) * math.sin((tmp_a+90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     lspickle.axis = axis
-    lspickle.size = (20, 320, 5)
+    lspickle.size = (20, ROBOT_WIDTH, 5)
 
-    rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*70) * math.cos((robot_a+90)*math.pi/180),
-                    robot_y - AREA_Y/2 + (robot_rspickle*70) * math.sin((robot_a+90)*math.pi/180),
+    robot_rspickle = 2 # XXX
+    rspickle.pos = (tmp_x + (robot_rspickle*60) * math.cos((tmp_a-90)*math.pi/180),
+                    tmp_y + (robot_rspickle*60) * math.sin((tmp_a-90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     rspickle.axis = axis
-    rspickle.size = (20, 320, 5)
+    rspickle.size = (20, ROBOT_WIDTH, 5)
 
     # save position
-    save_pos.append((robot.pos.x, robot.pos, robot_a))
+    save_pos.append((robot.pos.x, robot.pos.y, tmp_a))
 
     pos = robot.pos.x, robot.pos.y, 0.3
     if pos != last_pos:
@@ -297,7 +322,8 @@ def silent_mkfifo(f):
     except:
         pass
 
-init_corn_table(random.randint(0,8), random.randint(0,3))
+#init_corn_table(random.randint(0,8), random.randint(0,3))
+init_corn_table(0, 0)
 waypoints = init_waypoints()
 toggle_obj_disp()
 
@@ -308,31 +334,41 @@ while True:
         fr = open("/tmp/.robot_sim2dis", "r")
         fw = open("/tmp/.robot_dis2sim", "w", 0)
         while True:
+            m = None
             l = fr.readline()
-            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()
-            m = re.match("ballboard=%s"%(INT), l)
-            if m:
-                print int(m.groups()[0])
-            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
+
+            # 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 "ballboard: %d"%(int(m.groups()[0]))
+
+            # parse cobboard
+            if not m:
+                m = re.match("cobboard=%s,%s"%(INT,INT), l)
+                if m:
+                    print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1]))
+                    side = int(m.groups()[0])
+                    flags = int(m.groups()[1])
+                    if side == 0:
+                        if (flags & 0x01) == 0:
+                            robot_lspickle = 1
+                        else:
+                            robot_lspickle = 2
+                    else:
+                        if (flags & 0x01) == 0:
+                            robot_rspickle = 1
+                        else:
+                            robot_rspickle = 2
 
             if scene.kb.keys == 0:
                 continue
@@ -361,6 +397,8 @@ while True:
                 save()
             elif k == "h":
                 toggle_obj_disp()
+            elif k == "i":
+                toggle_color()
 
             # EOF
             if l == "":