Modify servo sample code