Servo controll

Under construction ...