Fix Servo driver and tests for angle protection