Code for servo demo (April 2013)