Added servotest.c