Added servo test code