Make work for servo control demo