add servo test code