projects/microb2010/mainboard/display.py

   1 import math, sys, time, os, random, re
   2 from visual import *
   3
   4 FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
   5 INT = "([-+]?[0-9][0-9]*)"
   6
   7 AREA_X = 3000.
   8 AREA_Y = 2100.
   9
  10 ROBOT_HEIGHT=5 # 350
  11 CORN_HEIGHT=5  # 150
  12
  13 area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
  14 areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
  15 area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
  16
  17 scene.autoscale=0
  18
  19 # all positions of robot every 5ms
  20 save_pos = []
  21
  22 robot = box(color=(0.4, 0.4, 0.4))
  23 lspickle = box(color=(0.4, 0.4, 0.4))
  24 rspickle = box(color=(0.4, 0.4, 0.4))
  25
  26 last_pos = (0.,0.,0.)
  27 hcenter_line = curve()
  28 hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
  29 vcenter_line = curve()
  30 vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
  31
  32 yellowarea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
  33 yellowareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , yellowarea)
  34 yellowarea_box = box(pos=(-AREA_X/2+250,-AREA_Y/2+250,0), size=yellowareasize, color=(1.0, 1.0, 0.0))
  35
  36 bluearea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
  37 blueareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , bluearea)
  38 bluearea_box = box(pos=(AREA_X/2-250,-AREA_Y/2+250,0), size=blueareasize, color=(0.0, 0.0, 1.0))
  39
  40 greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ]
  41 greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea)
  42 greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3))
  43
  44 def square(sz):
  45     sq = curve()
  46     sq.pos = [(-sz, -sz, 0.3),
  47               (-sz, sz, 0.3),
  48               (sz, sz, 0.3),
  49               (sz, -sz, 0.3),
  50               (-sz, -sz, 0.3),]
  51     return sq
  52
  53 sq1 = square(250)
  54 sq2 = square(500)
  55
  56 robot_x = 0.
  57 robot_y = 0.
  58 robot_a = 0.
  59 robot_lspickle = 0
  60 robot_rspickle = 0
  61 robot_trail = curve()
  62 robot_trail_list = []
  63 max_trail = 500
  64
  65 area_objects = []
  66
  67 OFFSET_CORN_X=150
  68 OFFSET_CORN_Y=222
  69 STEP_CORN_X=225
  70 STEP_CORN_Y=250
  71
  72 WAYPOINTS_NBX = 13
  73 WAYPOINTS_NBY = 8
  74
  75 TYPE_WAYPOINT=0
  76 TYPE_DANGEROUS=1
  77 TYPE_WHITE_CORN=2
  78 TYPE_BLACK_CORN=3
  79 TYPE_OBSTACLE=4
  80 TYPE_BALL=5
  81 TYPE_NEIGH=6
  82
  83 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
  84 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
  85 corn_table = [TYPE_WHITE_CORN]*18
  86
  87 corn_side_confs = [
  88     [ 1, 4 ],
  89     [ 0, 4 ],
  90     [ 2, 4 ],
  91     [ 2, 3 ],
  92     [ 0, 3 ],
  93     [ 1, 3 ],
  94     [ 1, 6 ],
  95     [ 0, 6 ],
  96     [ 2, 6 ],
  97     ]
  98 corn_center_confs = [
  99     [ 5, 8 ],
 100     [ 7, 8 ],
 101     [ 5, 9 ],
 102     [ 7, 8 ],
 103     ]
 104
 105 def pt2corn(i,j):
 106     if i == 0 and j == 2: return 0
 107     if i == 0 and j == 4: return 1
 108     if i == 0 and j == 6: return 2
 109     if i == 2 and j == 3: return 3
 110     if i == 2 and j == 5: return 4
 111     if i == 2 and j == 7: return 5
 112     if i == 4 and j == 4: return 6
 113     if i == 4 and j == 6: return 7
 114     if i == 6 and j == 5: return 8
 115     if i == 6 and j == 7: return 9
 116     if i == 8 and j == 4: return 10
 117     if i == 8 and j == 6: return 11
 118     if i == 10 and j == 3: return 12
 119     if i == 10 and j == 5: return 13
 120     if i == 10 and j == 7: return 14
 121     if i == 12 and j == 2: return 15
 122     if i == 12 and j == 4: return 16
 123     if i == 12 and j == 6: return 17
 124     return -1
 125
 126 def corn_get_sym(i):
 127     sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2]
 128     return sym[i]
 129
 130 def init_corn_table(conf_side, conf_center):
 131     global corn_table, corn_side_confs, corn_center_confs
 132     print "confs = %d, %d"%(conf_side, conf_center)
 133     for i in range(18):
 134         if i in corn_side_confs[conf_side]:
 135             corn_table[i] = TYPE_BLACK_CORN
 136             continue
 137         if corn_get_sym(i) in corn_side_confs[conf_side]:
 138             corn_table[i] = TYPE_BLACK_CORN
 139             continue
 140         if i in corn_center_confs[conf_center]:
 141             corn_table[i] = TYPE_BLACK_CORN
 142             continue
 143         if corn_get_sym(i) in corn_center_confs[conf_center]:
 144             corn_table[i] = TYPE_BLACK_CORN
 145             continue
 146         corn_table[i] = TYPE_WHITE_CORN
 147
 148 def init_waypoints():
 149     global waypoints, corn_table
 150
 151     for i in range(WAYPOINTS_NBX):
 152         for j in range(WAYPOINTS_NBY):
 153             # corn
 154             c = pt2corn(i, j)
 155             if c >= 0:
 156                 waypoints[i][j] = corn_table[c]
 157                 continue
 158
 159             # balls
 160             if (i & 1) == 0 and j > 3:
 161                 waypoints[i][j] = TYPE_BALL
 162                 continue
 163             if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2:
 164                 waypoints[i][j] = TYPE_BALL
 165                 continue
 166
 167             # too close of border
 168             if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
 169                 waypoints[i][j] = TYPE_OBSTACLE
 170                 continue
 171             # hill
 172             if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
 173                 waypoints[i][j] = TYPE_OBSTACLE
 174                 continue
 175             # dangerous points
 176             if i == 0 or i == WAYPOINTS_NBX-1:
 177                 waypoints[i][j] = TYPE_DANGEROUS
 178                 continue
 179             if (i&1) == 0 and j == WAYPOINTS_NBY-1:
 180                 waypoints[i][j] = TYPE_DANGEROUS
 181                 continue
 182
 183             waypoints[i][j] = TYPE_WAYPOINT
 184
 185         print i, waypoints[i]
 186     return waypoints
 187
 188
 189 def toggle_obj_disp():
 190     global area_objects
 191
 192     """
 193     if area_objects == []:
 194         c = sphere(radius=5, color=(0., 0.,1.),
 195                    pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5))
 196         area_objects.append(c)
 197         c = sphere(radius=5, color=(0., 0.,1.),
 198                    pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5))
 199         area_objects.append(c)
 200         c = sphere(radius=5, color=(0., 0.,1.),
 201                    pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5))
 202         area_objects.append(c)
 203         c = sphere(radius=5, color=(0., 0.,1.),
 204                    pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5))
 205         area_objects.append(c)
 206 """
 207     if area_objects == []:
 208         i = 0
 209         j = 0
 210         x = OFFSET_CORN_X
 211         while x < 3000:
 212             if (i & 1) == 0:
 213                 y = OFFSET_CORN_Y
 214             else:
 215                 y = OFFSET_CORN_Y + STEP_CORN_Y/2
 216             j = 0
 217             while y < 2100:
 218                 print x,y
 219                 if waypoints[i][j] == TYPE_WHITE_CORN:
 220                     c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
 221                                  radius=25, color=(0.8,0.8,0.8),
 222                                  pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
 223                     area_objects.append(c)
 224                 elif waypoints[i][j] == TYPE_BLACK_CORN:
 225                     c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
 226                                  radius=25, color=(0.2,0.2,0.2),
 227                                  pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
 228                     area_objects.append(c)
 229                 elif waypoints[i][j] == TYPE_BALL:
 230                     c = sphere(radius=50, color=(1., 0.,0.),
 231                                pos=(x-AREA_X/2,y-AREA_Y/2,50))
 232                     area_objects.append(c)
 233                 else:
 234                     c = sphere(radius=5, color=(0., 0.,1.),
 235                                pos=(x-AREA_X/2,y-AREA_Y/2,5))
 236                     area_objects.append(c)
 237                 j += 1
 238                 y += STEP_CORN_Y
 239             i += 1
 240             x += STEP_CORN_X
 241     else:
 242         for o in area_objects:
 243             if o.visible:
 244                 o.visible = 0
 245             else:
 246                 o.visible = 1
 247
 248
 249 def set_robot():
 250     global robot, last_pos, robot_trail, robot_trail_list
 251     global save_pos, robot_x, robot_y, robot_a
 252
 253     axis = (math.cos(robot_a*math.pi/180),
 254             math.sin(robot_a*math.pi/180),
 255             0)
 256
 257     robot.pos = (robot_x - AREA_X/2, robot_y - AREA_Y/2, ROBOT_HEIGHT/2)
 258     robot.axis = axis
 259     robot.size = (250, 320, ROBOT_HEIGHT)
 260
 261     lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a-90)*math.pi/180),
 262                     robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a-90)*math.pi/180),
 263                     ROBOT_HEIGHT/2)
 264     lspickle.axis = axis
 265     lspickle.size = (20, 320, 5)
 266
 267     rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a+90)*math.pi/180),
 268                     robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a+90)*math.pi/180),
 269                     ROBOT_HEIGHT/2)
 270     rspickle.axis = axis
 271     rspickle.size = (20, 320, 5)
 272
 273     # save position
 274     save_pos.append((robot.pos.x, robot.pos, robot_a))
 275
 276     pos = robot.pos.x, robot.pos.y, 0.3
 277     if pos != last_pos:
 278         robot_trail_list.append(pos)
 279         last_pos = pos
 280     robot_trail_l = len(robot_trail_list)
 281     if robot_trail_l > max_trail:
 282         robot_trail_list = robot_trail_list[robot_trail_l - max_trail:]
 283     robot_trail.pos = robot_trail_list
 284
 285 def graph():
 286     pass
 287
 288 def save():
 289     f = open("/tmp/robot_save", "w")
 290     for p in save_pos:
 291         f.write("%f %f %f\n"%(p[0], p[1], p[2]))
 292     f.close()
 293
 294 def silent_mkfifo(f):
 295     try:
 296         os.mkfifo(f)
 297     except:
 298         pass
 299
 300 init_corn_table(random.randint(0,8), random.randint(0,3))
 301 waypoints = init_waypoints()
 302 toggle_obj_disp()
 303
 304 while True:
 305     silent_mkfifo("/tmp/.robot_sim2dis")
 306     silent_mkfifo("/tmp/.robot_dis2sim")
 307     while True:
 308         fr = open("/tmp/.robot_sim2dis", "r")
 309         fw = open("/tmp/.robot_dis2sim", "w", 0)
 310         while True:
 311             m = None
 312             l = fr.readline()
 313
 314             # parse position
 315             if not m:
 316                 m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l)
 317                 if m:
 318                     robot_x = int(m.groups()[0])
 319                     robot_y = int(m.groups()[1])
 320                     robot_a = int(m.groups()[2])
 321                     set_robot()
 322
 323             # parse ballboard
 324             if not m:
 325                 m = re.match("ballboard=%s"%(INT), l)
 326                 if m:
 327                     print int(m.groups()[0])
 328
 329             # parse cobboard
 330             if not m:
 331                 m = re.match("cobboard=%s"%(INT), l)
 332                 if m:
 333                     mode = int(m.groups()[0])
 334                     if (mode & 0x01) == 0:
 335                         robot_lspickle = 0
 336                     elif (mode & 0x02) == 0:
 337                         robot_lspickle = 1
 338                     else:
 339                         robot_lspickle = 2
 340                     if (mode & 0x04) == 0:
 341                         robot_rspickle = 0
 342                     elif (mode & 0x08) == 0:
 343                         robot_rspickle = 1
 344                     else:
 345                         robot_rspickle = 2
 346
 347             if scene.kb.keys == 0:
 348                 continue
 349
 350             k = scene.kb.getkey()
 351             x,y,z = scene.center
 352             if k == "left":
 353                 scene.center = x-10,y,z
 354             elif k == "right":
 355                 scene.center = x+10,y,z
 356             elif k == "up":
 357                 scene.center = x,y+10,z
 358             elif k == "down":
 359                 scene.center = x,y-10,z
 360             elif k == "l":
 361                 fw.write("l")
 362             elif k == "r":
 363                 fw.write("r")
 364             elif k == "c":
 365                 robot_trail_list = []
 366             elif k == "x":
 367                 save_pos = []
 368             elif k == "g":
 369                 graph()
 370             elif k == "s":
 371                 save()
 372             elif k == "h":
 373                 toggle_obj_disp()
 374
 375             # EOF
 376             if l == "":
 377                 break
 378
 379         fr.close()
 380         fw.close()
 381