Hay alguna cosa que no entiendo. No sé lo que quieres hacer con el barrido del sensor en el servo. Creo que lo primero deberías explicar bien qué es lo que quieres que haga tu robot, éste es el punto de partida que tienes que tener muy claro. Por ejemplo el barrido que haces está con delay así que sólo tomas medidas del sensor cuando el servo está en las dos posiciones extremas. ¿Es eso lo que quieres?
Yo en principio veo dos formas de funcionamiento. Una fácil sería dejar el sensor fijo mirando al frente. Entonces empezamos a mover el robot según los comandos que envías y siempre que el sensor no detecte obstáculo. En cuanto detecte un obstáculo hay que hacer girar el robot hasta que vea vía libre. En este momento también hay que decidir si el robot toma una instrucción de movimiento automática o si ejecuta el movimiento que se le dio con la última instrucción.
La otra opción, instalando el sensor en el servo, la veo más para implementar un funcionamiento más autónomo que guiado. Podría ser programar el robot para estar continuamente tomando lecturas el sensor en los 360 grados y hacer que el robot se oriente hacia donde detecte que hay más espacio. Esto lo veo ya algo complicado de programar más que nada porque por el servo sabes la posición en la que se detecta el mayor espacio pero después no tienes un control exacto de lo que gira el robot para ponerlo en la dirección exacta.
También podría ser que el robot siguiera las instrucciones enviadas y cuando detectara un obstáculo se pare, haga barrido con el sensor los 360 grados, detecte la dirección de máxima distancia, posiciones el robot en esa dirección y siga.
¿Qué opciones de éstas o alguna otra es la que quieres hacer?