Add C example for servo controlling