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()
+