Create servotest.c

Delete this later