test_go_to_point.py 文件源码

def goToPoint(point, speed):
    lt = Timer()
    angle, dist = angleDistToPoint(point)
    #print angle, dist
    rpb202.motionCtrl.turnToAngle(angle)
    while dist > 100:
        rpb202.motionCtrl.forwardAngle(speed, angle)
        lt.sleepToElapsed(.02)
        angle, dist = angleDistToPoint(point)
    rpb202.motionCtrl.forwardDist(speed, dist, decel=True)
    rpb202.stop()
    print rpb202.odometer.getPosXY()