Fix angle saftey in Servo driver