revised servo_main.c