X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fdisplay.py;h=5700edce1f51dd7cc066c3bd7a6d8be868ca41f9;hp=f89580cad18aed5395e54893341e199098e96ba7;hb=b56f8565015ec12af0e8df461f8dd52d07f59dd7;hpb=092caa88280f71fb58e5938114304fe2e94d6855

diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index f89580c..5700edc 100644
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -9,9 +9,10 @@ AREA_Y = 2100.
 
 ROBOT_HEIGHT=5 # 350
 CORN_HEIGHT=5  # 150
+BALL_CYLINDER=1 # 0
 
 ROBOT_WIDTH=320
-ROBOT_LENGTH=360
+ROBOT_LENGTH=250
 
 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)
@@ -26,6 +27,8 @@ 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))
 
+opp = box(color=(0.7, 0.2, 0.2))
+
 last_pos = (0.,0.,0.)
 hcenter_line = curve()
 hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
@@ -63,8 +66,10 @@ sq2 = square(500)
 robot_x = 0.
 robot_y = 0.
 robot_a = 0.
-robot_lspickle = 0
-robot_rspickle = 0
+robot_lspickle_deployed = 0
+robot_rspickle_deployed = 0
+robot_lspickle_autoharvest = 0
+robot_rspickle_autoharvest = 0
 robot_trail = curve()
 robot_trail_list = []
 max_trail = 500
@@ -234,8 +239,15 @@ def toggle_obj_disp():
                                  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))
+                    if BALL_CYLINDER == 1:
+                        c = cylinder(axis=(0,0,1), radius=50,
+                                     length=CORN_HEIGHT,
+                                     color=(1., 0.,0.),
+                                     pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
+                    else:
+                        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.),
@@ -260,6 +272,10 @@ def toggle_color():
     else:
         color = YELLOW
 
+def set_opp(x, y):
+    opp.size = (300, 300, ROBOT_HEIGHT)
+    opp.pos = (x, y, ROBOT_HEIGHT/2)
+
 def set_robot():
     global robot, last_pos, robot_trail, robot_trail_list
     global save_pos, robot_x, robot_y, robot_a
@@ -281,19 +297,25 @@ def set_robot():
     robot.axis = axis
     robot.size = (ROBOT_LENGTH, ROBOT_WIDTH, ROBOT_HEIGHT)
 
-    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),
+    lspickle.pos = (tmp_x + (robot_lspickle_deployed*60) * math.cos((tmp_a+90)*math.pi/180),
+                    tmp_y + (robot_lspickle_deployed*60) * math.sin((tmp_a+90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     lspickle.axis = axis
     lspickle.size = (20, ROBOT_WIDTH, 5)
+    if robot_lspickle_autoharvest:
+        lspickle.color = (1, 0, 0)
+    else:
+        lspickle.color = (0.4, 0.4, 0.4)
 
-    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),
+    rspickle.pos = (tmp_x + (robot_rspickle_deployed*60) * math.cos((tmp_a-90)*math.pi/180),
+                    tmp_y + (robot_rspickle_deployed*60) * math.sin((tmp_a-90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     rspickle.axis = axis
     rspickle.size = (20, ROBOT_WIDTH, 5)
+    if robot_rspickle_autoharvest:
+        rspickle.color = (1, 0, 0)
+    else:
+        rspickle.color = (0.4, 0.4, 0.4)
 
     # save position
     save_pos.append((robot.pos.x, robot.pos.y, tmp_a))
@@ -356,19 +378,26 @@ while True:
             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]))
+                    #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
+                        robot_lspickle_deployed = ((flags & 1) * 2)
+                        robot_lspickle_autoharvest = ((flags & 2) != 0)
                     else:
-                        if (flags & 0x01) == 0:
-                            robot_rspickle = 1
-                        else:
-                            robot_rspickle = 2
+                        robot_rspickle_deployed = ((flags & 1) * 2)
+                        robot_rspickle_autoharvest = ((flags & 2) != 0)
+
+            if scene.mouse.events != 0:
+                oppx, oppy, oppz = scene.mouse.getevent().project(normal=(0,0,1))
+                set_opp(oppx, oppy)
+                try:
+                    if color == YELLOW:
+                        fw.write("opp %d %d"%(int(oppx + 1500), int(oppy + 1050)))
+                    else:
+                        fw.write("opp %d %d"%(int(1500 - oppx), int(1050 - oppy)))
+                except:
+                    print "not connected"
 
             if scene.kb.keys == 0:
                 continue
@@ -399,6 +428,8 @@ while True:
                 toggle_obj_disp()
             elif k == "i":
                 toggle_color()
+            else:
+                print k
 
             # EOF
             if l == "":