Follow Line

El objetivo de está práctica era realizar un sigue líneas (follow line) en un robot que empleaba una placa Arduino Uno conectada en serie con una ESP que se encarga de realizar comunicación IoT, enviando diferentes tipos de mensajes según el comportamiento del robot:

Para realizar el movimiento básico del robot, tomamos los valores de los tres sensores infrarojos (izquierdo, central y derecho) con analogRead(), obteniendo para cada uno de ellos un valor entre 0 y 1023. A más valor obtenido, más oscura es la zona en la que se encuentra posado el robot.

Para conseguir que el robot siguiese la lína negra del circuito oficial (y de otros circuitos de prueba), establecimos una constante llamada RECTILINEAR_VALUE a modo de umbral para determinar si el robot se encuentra encima de la línea o no. Usando esta constante y los sensores infrarrojos, podemos determinar que:

• Si el valor medido por el sensor central es mayor que el umbral y los de los lados es inferior a dicho umbral, podemos considerar que el robot se encuentra encima de la línea en forma recta y no hay que girar.
  

• Si el valor del sensor izquierdo es mayor al umbral y los otros dos son inferiores al umbral, la línea se está "desplazando" hacia la izquierda y es necesario que el robot gire a la izquierda para seguir la línea

• Si el valor del sensor derecho es mayor al umbral y los otros dos son inferiores al umbral, la línea se está "desplazando" hacia la derecha y es necesario que el robot gire a la derecha para seguir la línea.

  Con el objetivo de conseguir un comportamiento más robusto en el sigue-líneas y en las situaciones en las que debe girar o no, determinamos también que:

  • Seguiremos en línea recta si el valor del medio es mayor que la suma de los valores izquierdo y derecho, aun siendo este valor menor al umbral.

  • Si la suma del izquierdo más el central es mayor que el derecho o el valor medido por el izquierdo es mayor que la suma de los otros dos (aun siendo este valor menor que el umbral), consideraremos que hay que girar a la izquierda.

  • Si la suma del derecho más el central es mayor que el izquierdo o el valor medido por el derecho es mayor que la suma de los otros dos (aun siendo este valor menor que el umbral), consideraremos que hay que girar a la derecha.

   Todas estas decisiones de seguir de frente o girar hacia un lado o hacia otro se realizan con las funciones booleanas de is_not_moving(), is_moving_right() e is_moving_left().

BÚSQUEDA DE LÍNEA PERDIDA:

En el caso en el que ninguna de las anteriores funciones devuelva true (los tres sensores dan valores inferiores al umbral), estaríamos ante el caso en el que el robot está completamente fuera de la línea. Para afrontar este caso implementamos un sistema de recuperación de línea perdida. Cada vez que el robot decida girar hacia un lado u otro actualizaremos en la variable last_operation la última operación realizada (TURN_LEFT o TURN_RIGHT).

 De este modo, si el robot se encuentra fuera de la línea y su última acción fue girar a la izquierda, el robot girará a la derecha para compensar el último movimiento y encontrar la línea. Si la última acción fue girar a la derecha, el robot girará a la izquierda hasta encontrar la línea.

 CONTROL DE VELOCIDADES DEL ROBOT:

Durante la realización del circuito, el robot irá modificando su velocidad (no empleamos un Movimiento Uniforme) en función de la acción que tome. Cada vez que el robot entre en la función go_forward() para ir en línea rectá se incrementará en uno la velocidad, mientras que si entra en las funciones de giro (turn_left() o turn_right()), decrementará la velocidad (la otra rueda estára a cero para conseguir el movimiento circular de giro) en un valor establecido por VEL_DECREASE_FACTOR.

Nota que durante los giros empleamos una velocidad fija de giro (ANG_VELOCITY), pero el decremento de velocidad lineal hace que al salir de una curva comience de nuevo a acelerar de forma progresiva y que no salga disparado de la curva. 

Además, establecemos una velocidad lineal máxima y mínima (MIN_VELOCITY y MAX_VELOCITY) para permitir mayor controlabilidad del movimiento del robot.

DETECCIÓN DE OBSTÁCULOS:

Al finalizar una vuelta completa del circuito, el robot se encontrará con un obstáculo que tendrá en frente y deberá detenerse a entre 5 y 8 centímetros de dicho obstáculo. Para ello, en nuestro programa creamos un hilo que va ejecutandose cada USSENSOR_INTERVAL milisegundos y va comprobando con las medidas tomadas por el sensor de ultrasonidos si tiene algún obstáculo a esa distancia. En caso de que sea así, el robot se detendrá.

Importante: A mayor frecuencia de iteración del hilo que se encarga de tomar las medidas del ultrasonido, menor será la reactividad que obtengamos en el resto del comportamiento del programa. Como es más prioritario conseguir mayor reactividad en el sigue-líneas por encima de la detección de obstáculos, establecemos el USSENSOR_INTERVAL=300 milisegundos y ponemos que el valor obtenido del obstáculo que haga que se pare sea entre 0 y 14 centímetros (en vez de entre 5 y 8 que es como debe pararse realmente). Con esto conseguimos ajustar francamente bien que el robot se pare en el intervalo que se especifica.

Comunicación MQTT

Durante todo el seguimiento de la linea el robot deberá ir publicando una serie de mensajes dando información de lo que está sucediendo. Estos mensajes serán publicados por la esp32 aprovechando su modulo WiFi en el topic "/SETR/2022/2/" a través del protocolo de comunicación MQTT. La esp recibirá el tipo de mensaje que tiene que enviar a través del puerto serie que la comunica con el Arduino. En total hay 8 tipos de mensajes:

  - PING

  - START_LAP

  - END_LAP

  - OBSTACLE_DETECTED

  - LINE_LOSE

  - INIT_LINE_SEARCH

  - STOP_LINE_SEARCH

  - LINE_FIND

Tenemos dos formatos en function del mensaje que estamos enviando:

- Para PING y END_LINE:    

{
	"team_name": "$TU_NOMBRE_DE_EQUIPO ",
	"id": "$ID_EQUIPO",
	"action": "current_action",
        "time": 000000
}

- Para el resto:

{
	"team_name": "$TU_NOMBRE_DE_EQUIPO ",
	"id": "$ID_EQUIPO",
	"action": "current_action"
}

Como podemos fijarnos la única diferencia entre unos y otros radica en el campo time. Para calcular este campo la esp32 almacenara una "time_stamp" del tiempo que tarda el arduino en enviar el primer mensaje START_LAP. A partir de ahí el calculo de time sera simplemente: "millis() - connect_time_ms".

Todos los mensajes se enviarán en formato JSON al tratarse de un estándar muy extendido que nos permitirá la fácil adaptación con otras aplicaciones.

En la comunicación esp32, arduino, servidor podemos distinguir dos partes:

- Fase de conexión:

En esta fase la esp32 se conecta al wifi para posteriormente conectarse al servidor por mqtt. Una vez se establece esta conexión comenzara a enviar un mensaje de aviso(en nuestro caso "1") al arduino por el puerto serie para indicarle que la conexión esta realizada y que puede comenzar su ejecución cuando quiera. Una vez el arduino reciba este mensaje responderá con un START_LAP dando inicio al comportamiento  follow_line y a la siguiente fase de comunicación.

- Fase de ejecución:

Una vez se ha comenzado el movimiento, el arduino ira indicando por el puerto serie a la esp el mensaje que desea enviar, esta tomara el tipo de mensaje, lo traducirá a formato JSON y lo publicará en el topic previamente mencionado.

VÍDEO DEMOSTRATIVO DEL RESULTADO: