finished servoGoto funcion