Improved servo_control

Some minor changes/fixes, added upper and lower bonds and linear
interpolation