Dado que este proyecto conlleva una cierta complejidad en la programación, vamos a dejar unas pistas para ayudar a programar la fase de búsqueda y acercamiento.

Ahora vamos a hacer que el robot haga un giro de 180º sobre sí mimo y durante este movimiento iremos comprobando si la medida que nos da el sensor de US es menor que los 256 que hemos puesto como valor inicial. En el caso de que la medida proporcionada por el US sea menor se almacenará en la variable distancia. Como durante todo el bucle va a estar comprobando esta situación, si encuentra algún objeto a una distancia menor, almacenará la distancia de ese objeto más cercano.

Para conocer en que punto del giro se encuenta el objeto, se almacenará el la variable Dirección el valor del sensor de rotación en ese momento. El bucle termina cuando el sensor de rotación ha recorrido el ángulo necesario para que el robot gire los 180º sobre sí mismo. Este dato lo tenéis que calcular vosotros.

El programa sería algo así:

Una vez el robot sabe donde está el objeto más cercano por medio de las variables Distancia y Dirección, tiene que girar hasta situarse frente a él y acercarse a él.

En la última parte del programa Localizar objeto vamos a posicionar el robot justo delante del objeto detectado. Para ello vamos a restar el valor de la variable Dirección del valor del sensor de rotación. De esta forma sabremos los grados que hay que retroceder. Este valor se le proporciona al motor C y el programa espera a que la función finalice mientras que el motor B también gira en sentido inverso.

Es importante observar que si no se ha detectado ningún objeto, la variable Dirección no se ha modificado y por tanto sigue en 0. En este caso el robot retrocederá los 1000 grados girados en el rastreo de objetos situándose en la posición inicial.