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