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