Servo func and small adjustment

関数追加
Merge branch 'Servo_Arm'