restore first circle algo + hostsim work

[aversive.git] / projects / microb2010 / tests / hostsim / display.py

diff --git a/projects/microb2010/tests/hostsim/display.py b/projects/microb2010/tests/hostsim/display.py
index a6c90e7..d21cc9e 100644 (file)
--- a/projects/microb2010/tests/hostsim/display.py
+++ b/projects/microb2010/tests/hostsim/display.py
@@ -10,53 +10,113 @@ area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
 
 scene.autoscale=0
 
+# all positions of robot every 5ms
+save_pos = []
+
 robot = box(pos = (0, 0, 150),
             size = (300,300,300),
             color = (1, 0, 0) )
 
 last_pos = robot.pos.x, robot.pos.y, robot.pos.z
+hcenter_line = curve()
+hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
+vcenter_line = curve()
+vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
+
+def square(sz):
+    sq = curve()
+    sq.pos = [(-sz, -sz, 0.3),
+              (-sz, sz, 0.3),
+              (sz, sz, 0.3),
+              (sz, -sz, 0.3),
+              (-sz, -sz, 0.3),]
+    return sq
+
+sq1 = square(250)
+sq1 = square(500)
+
 robot_trail = curve()
+robot_trail_list = []
 max_trail = 500
 
 def set_robot(x, y, a):
-    global robot, last_pos
+    global robot, last_pos, robot_trail, robot_trail_list
+    global save_pos
+
     robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150)
     robot.axis = (math.cos(a*math.pi/180) * 300,
                   math.sin(a*math.pi/180) * 300,
                   0)
 
-    pos = robot.pos.x, robot.pos.y, robot.pos.z
+    # save position
+    save_pos.append(robot.pos.x, robot.pos, a)
+
+    pos = robot.pos.x, robot.pos.y, 0.3
     if pos != last_pos:
-        robot_trail.append(pos)
+        robot_trail_list.append(pos)
         last_pos = pos
-    robot_trail_l = len(robot_trail.pos)
-    if robot_trail_l> max_trail:
-        robot_trail.pos = robot_trail.pos[robot_trail_l - max_trail:]
+    robot_trail_l = len(robot_trail_list)
+    if robot_trail_l > max_trail:
+        robot_trail_list = robot_trail_list[robot_trail_l - max_trail:]
+    robot_trail.pos = robot_trail_list
 
-while True:
+def graph():
+    pass
+
+def save():
+    f = open("/tmp/robot_save", "w")
+    for p in save_pos:
+        f.write("%f %f %f\n"%(p[0], p[1], p[2]))
+    f.close()
+
+def silent_mkfifo(f):
     try:
-        os.mkfifo("/tmp/.robot")
+        os.mkfifo(f)
     except:
         pass
+
+while True:
+    silent_mkfifo("/tmp/.robot_sim2dis")
+    silent_mkfifo("/tmp/.robot_dis2sim")
     while True:
-        f = open("/tmp/.robot")
+        fr = open("/tmp/.robot_sim2dis", "r")
+        fw = open("/tmp/.robot_dis2sim", "w", 0)
         while True:
-            l = f.readline()
+            l = fr.readline()
             if l == "":
                 break
-            x,y,a = map(lambda x:int(x), l[:-1].split(" "))
-            set_robot(x,y,a)
-        f.close()
-
-    """
-    k = scene.kb.getkey()
-    x,y,z = scene.center
-    if k == "left":
-        scene.center = x-10,y,z
-    elif k == "right":
-        scene.center = x+10,y,z
-    elif k == "up":
-        scene.center = x,y+10,z
-    elif k == "down":
-        scene.center = x,y-10,z
-    """
+            try:
+                x,y,a = map(lambda x:int(x), l[:-1].split(" "))
+                set_robot(x,y,a)
+            except ValueError:
+                pass
+
+            if scene.kb.keys == 0:
+                continue
+
+            k = scene.kb.getkey()
+            x,y,z = scene.center
+            if k == "left":
+                scene.center = x-10,y,z
+            elif k == "right":
+                scene.center = x+10,y,z
+            elif k == "up":
+                scene.center = x,y+10,z
+            elif k == "down":
+                scene.center = x,y-10,z
+            elif k == "l":
+                fw.write("l")
+            elif k == "r":
+                fw.write("r")
+            elif k == "c":
+                robot_trail_list = []
+            elif k == "x":
+                save_pos = []
+            elif k == "g":
+                graph()
+            elif k == "s":
+                save()
+
+        fr.close()
+        fw.close()
+