Updated ServoControl.c