Implemented servo control