modified servo demo code