Added servo set function