def nav():
r = Robot()
pose = utils.home_pose
destination = Point(x=0, y=0)
destination_x = raw_input('enter destination x')
destination.x = int(destination_x)
destination_y = raw_input('enter destination y')
destination.y = int(destination_y)
distance = utils.wu_to_point(destination, pose)
angle = utils.rotation_to_point(destination, pose)
'''go to point from pose'''
r.r.rotate(angle)
time.sleep(2)
r.r.travel(distance)
def lap():
loop = [Point(x=x1,y=y1) for x1,y1 in LOOP]
r = Robot(plotting=True)
pose = utils.home_pose
last_time = time.time()
p = Process(target=update_loop, args=[r, 2])
p.daemon=True
p.start()
for point in loop:
print point.x, point.y
print 'pose: %d, %d, %.2f' % (pose.x, pose.y, pose.theta)
distance = utils.wu_to_point(point, pose)
angle = utils.rotation_to_point(point, pose)
r.r.rotate(angle)
time.sleep(3)
r.r.travel(distance)
time.sleep(3)
pose = Pose(x=point.x, y=point.y, theta=pose.theta-angle)
