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