projects/microb2010/mainboard/display.py

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