Fix stopping points for servos. Add code to turn sonar servo.