uarm_core.py 文件源码

def readCurrentAngles():
    ra = {}
    ra['s1'] = uarm.get_servo_angle(0)
    ra['s2'] = uarm.get_servo_angle(1)
    ra['s3'] = uarm.get_servo_angle(2)
    ra['s4'] = uarm.get_servo_angle(3)

    print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(ra['s1'], ra['s2'],ra['s3'],ra['s4'])

    rospy.set_param('servo_1',ra['s1'])
    rospy.set_param('servo_2',ra['s2'])
    rospy.set_param('servo_3',ra['s3'])
    rospy.set_param('servo_4',ra['s4'])

# Read stopper function