static beacon - offset
[aversive.git] / 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 j < 7:
178                 waypoints[i][j] = TYPE_BALL
179                 continue
180
181             # too close of border
182             if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
183                 waypoints[i][j] = TYPE_OBSTACLE
184                 continue
185             # hill
186             if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
187                 waypoints[i][j] = TYPE_OBSTACLE
188                 continue
189             # dangerous points
190             if i == 0 or i == WAYPOINTS_NBX-1:
191                 waypoints[i][j] = TYPE_DANGEROUS
192                 continue
193             if (i&1) == 0 and j == WAYPOINTS_NBY-1:
194                 waypoints[i][j] = TYPE_DANGEROUS
195                 continue
196
197             waypoints[i][j] = TYPE_WAYPOINT
198
199         print i, waypoints[i]
200     return waypoints
201
202
203 def toggle_obj_disp():
204     global area_objects
205
206     """
207     if area_objects == []:
208         c = sphere(radius=5, color=(0., 0.,1.),
209                    pos=(1238.-AREA_X/2, 1313.-AREA_Y/2, 5))
210         area_objects.append(c)
211         c = sphere(radius=5, color=(0., 0.,1.),
212                    pos=(1364.-AREA_X/2, 1097.-AREA_Y/2, 5))
213         area_objects.append(c)
214         c = sphere(radius=5, color=(0., 0.,1.),
215                    pos=(1453.-AREA_X/2, 1176.-AREA_Y/2, 5))
216         area_objects.append(c)
217         c = sphere(radius=5, color=(0., 0.,1.),
218                    pos=(1109.-AREA_X/2, 1050.-AREA_Y/2, 5))
219         area_objects.append(c)
220 """
221     if area_objects == []:
222         i = 0
223         j = 0
224         x = OFFSET_CORN_X
225         while x < 3000:
226             if (i & 1) == 0:
227                 y = OFFSET_CORN_Y
228             else:
229                 y = OFFSET_CORN_Y + STEP_CORN_Y/2
230             j = 0
231             while y < 2100:
232                 print x,y
233                 if waypoints[i][j] == TYPE_WHITE_CORN:
234                     c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
235                                  radius=25, color=(0.8,0.8,0.8),
236                                  pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
237                     area_objects.append(c)
238                 elif waypoints[i][j] == TYPE_BLACK_CORN:
239                     c = cylinder(axis=(0,0,1), length=CORN_HEIGHT,
240                                  radius=25, color=(0.2,0.2,0.2),
241                                  pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
242                     area_objects.append(c)
243                 elif waypoints[i][j] == TYPE_BALL:
244                     if BALL_CYLINDER == 1:
245                         c = cylinder(axis=(0,0,1), radius=50,
246                                      length=CORN_HEIGHT,
247                                      color=(1., 0.,0.),
248                                      pos=(x-AREA_X/2,y-AREA_Y/2,CORN_HEIGHT/2))
249                     else:
250                         c = sphere(radius=50, color=(1., 0.,0.),
251                                    pos=(x-AREA_X/2,y-AREA_Y/2,50))
252
253                     area_objects.append(c)
254                 else:
255                     c = sphere(radius=5, color=(0., 0.,1.),
256                                pos=(x-AREA_X/2,y-AREA_Y/2,5))
257                     area_objects.append(c)
258                 j += 1
259                 y += STEP_CORN_Y
260             i += 1
261             x += STEP_CORN_X
262     else:
263         for o in area_objects:
264             if o.visible:
265                 o.visible = 0
266             else:
267                 o.visible = 1
268
269 def toggle_color():
270     global color
271     global BLUE, YELLOW
272     if color == YELLOW:
273         color = BLUE
274     else:
275         color = YELLOW
276
277 def set_opp(x, y):
278     opp.size = (300, 300, ROBOT_HEIGHT)
279     opp.pos = (x, y, ROBOT_HEIGHT/2)
280
281 def set_robot():
282     global robot, last_pos, robot_trail, robot_trail_list
283     global save_pos, robot_x, robot_y, robot_a
284
285     if color == YELLOW:
286         tmp_x = robot_x - AREA_X/2
287         tmp_y = robot_y - AREA_Y/2
288         tmp_a = robot_a
289     else:
290         tmp_x = -robot_x + AREA_X/2
291         tmp_y = -robot_y + AREA_Y/2
292         tmp_a = robot_a
293
294     robot.pos = (tmp_x, tmp_y, ROBOT_HEIGHT/2)
295     axis = (math.cos(tmp_a*math.pi/180),
296             math.sin(tmp_a*math.pi/180),
297             0)
298
299     robot.axis = axis
300     robot.size = (ROBOT_LENGTH, ROBOT_WIDTH, ROBOT_HEIGHT)
301
302     lspickle.pos = (tmp_x + (robot_lspickle_deployed*60) * math.cos((tmp_a+90)*math.pi/180),
303                     tmp_y + (robot_lspickle_deployed*60) * math.sin((tmp_a+90)*math.pi/180),
304                     ROBOT_HEIGHT/2)
305     lspickle.axis = axis
306     lspickle.size = (20, ROBOT_WIDTH, 5)
307     if robot_lspickle_autoharvest:
308         lspickle.color = (0.2, 0.2, 1)
309     else:
310         lspickle.color = (0.4, 0.4, 0.4)
311
312     rspickle.pos = (tmp_x + (robot_rspickle_deployed*60) * math.cos((tmp_a-90)*math.pi/180),
313                     tmp_y + (robot_rspickle_deployed*60) * math.sin((tmp_a-90)*math.pi/180),
314                     ROBOT_HEIGHT/2)
315     rspickle.axis = axis
316     rspickle.size = (20, ROBOT_WIDTH, 5)
317     if robot_rspickle_autoharvest:
318         rspickle.color = (0.2, 0.2, 1)
319     else:
320         rspickle.color = (0.4, 0.4, 0.4)
321
322     # save position
323     save_pos.append((robot.pos.x, robot.pos.y, tmp_a))
324
325     pos = robot.pos.x, robot.pos.y, 0.3
326     if pos != last_pos:
327         robot_trail_list.append(pos)
328         last_pos = pos
329     robot_trail_l = len(robot_trail_list)
330     if robot_trail_l > max_trail:
331         robot_trail_list = robot_trail_list[robot_trail_l - max_trail:]
332     robot_trail.pos = robot_trail_list
333
334 def graph():
335     pass
336
337 def save():
338     f = open("/tmp/robot_save", "w")
339     for p in save_pos:
340         f.write("%f %f %f\n"%(p[0], p[1], p[2]))
341     f.close()
342
343 def silent_mkfifo(f):
344     try:
345         os.mkfifo(f)
346     except:
347         pass
348
349 #init_corn_table(random.randint(0,8), random.randint(0,3))
350 init_corn_table(0, 0)
351 waypoints = init_waypoints()
352 toggle_obj_disp()
353
354 while True:
355     silent_mkfifo("/tmp/.robot_sim2dis")
356     silent_mkfifo("/tmp/.robot_dis2sim")
357     while True:
358         fr = open("/tmp/.robot_sim2dis", "r")
359         fw = open("/tmp/.robot_dis2sim", "w", 0)
360         while True:
361             m = None
362             l = fr.readline()
363
364             # parse position
365             if not m:
366                 m = re.match("pos=%s,%s,%s"%(INT,INT,INT), l)
367                 if m:
368                     robot_x = int(m.groups()[0])
369                     robot_y = int(m.groups()[1])
370                     robot_a = int(m.groups()[2])
371                     set_robot()
372
373             # parse ballboard
374             if not m:
375                 m = re.match("ballboard=%s"%(INT), l)
376                 if m:
377                     print "ballboard: %d"%(int(m.groups()[0]))
378
379             # parse cobboard
380             if not m:
381                 m = re.match("cobboard=%s,%s"%(INT,INT), l)
382                 if m:
383                     print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1]))
384                     side = int(m.groups()[0])
385                     flags = int(m.groups()[1])
386                     if (side == 0 and color == YELLOW) or (side == 1 and color == BLUE):
387                         robot_lspickle_deployed = ((flags & 1) * 2)
388                         robot_lspickle_autoharvest = ((flags & 2) != 0)
389                     else:
390                         robot_rspickle_deployed = ((flags & 1) * 2)
391                         robot_rspickle_autoharvest = ((flags & 2) != 0)
392
393             if scene.mouse.events != 0:
394                 oppx, oppy, oppz = scene.mouse.getevent().project(normal=(0,0,1))
395                 set_opp(oppx, oppy)
396                 try:
397                     if color == YELLOW:
398                         fw.write("opp %d %d"%(int(oppx + 1500), int(oppy + 1050)))
399                     else:
400                         fw.write("opp %d %d"%(int(1500 - oppx), int(1050 - oppy)))
401                 except:
402                     print "not connected"
403
404             if scene.kb.keys == 0:
405                 continue
406
407             k = scene.kb.getkey()
408             x,y,z = scene.center
409             if k == "left":
410                 scene.center = x-10,y,z
411             elif k == "right":
412                 scene.center = x+10,y,z
413             elif k == "up":
414                 scene.center = x,y+10,z
415             elif k == "down":
416                 scene.center = x,y-10,z
417             elif k == "l":
418                 fw.write("l")
419             elif k == "r":
420                 fw.write("r")
421             elif k == "c":
422                 robot_trail_list = []
423             elif k == "x":
424                 save_pos = []
425             elif k == "g":
426                 graph()
427             elif k == "s":
428                 save()
429             elif k == "h":
430                 toggle_obj_disp()
431             elif k == "i":
432                 toggle_color()
433             else:
434                 print k
435
436             # EOF
437             if l == "":
438                 break
439
440         fr.close()
441         fw.close()
442