Servo driver updated to support read functions