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)