Fixed bug in Servo block