revise the servo_main.c