changed servo test code for debugging