Nuevo algoritmo de detección de contenedores

Como ya se pudo ver en el vídeo que publicamos hace unos días, tras la implementación de la brújula hemos mejorado los algoritmos que permitían a nuestro robot detectar contenedores y clasificar los residuos de una manera autónoma.

En esta entrada explicaremos con más detalle el algoritmo de detección, que forma parte del Behavior Mapping(); en la siguiente, nos centraremos en el de clasificación de residuos, definido en el Behavior ProcessData().

Behavior Mapping()

Como ya vimos en la entrada titulada “Software básico del robot (parte III)”, anteriormente este comportamiento hacía que el robot diese unas vueltas de reconocimiento y, cuando detectaba un contenedor, medíamos a través del tacómetro de los motores en qué posición se encontraba.

Con el nuevo algoritmo, medimos ángulos mediante la brújula I2C e introducimos algunas comprobaciones para evitar que se realicen falsas detecciones de contenedores.

En el siguiente diagrama de estados (hacer click sobre la imagen para ver en grande) se puede obtener una visión global del funcionamiento del algoritmo, que pasaremos a describir con más detalle estado a estado a continuación:

• Establecimiento de posición inicial.
  Inicialmente, el robot considera que delante tiene un objeto (la variable booleana “libre” toma el valor “false”) y gira hasta que el sensor de ultrasonidos no detecta nada (hasta tener “libre=true”). Las medidas del sensor de ultrasonidos se realizan de 5 en 5: si al menos 3 de ellas devuelven un valor de no detección, se considerará que no hay ningún contenedor delante y se cambiará el valor de “libre” a “true”. Los valores de no detección son -1 (quiere decir que ningún objeto es detectado por el sensor de ultrasonidos) y todos aquellos superiores a 90 (el objeto detectado se encuentran a más de 90 cm de nuestro robot).
  Hemos implementado esta fase inicial para evitar errores: si el robot tuviese delante un contenedor nada más empezar el mapeo, no habría podido detectar en qué ángulo comienza a haber contenedor y tampoco podría calcular a qué ángulo se encuentra el punto medio del contenedor.
  Cuando ya nos aseguramos de que no hay nada delante del robot, tomamos una primera lectura de la brújula para saber cuál es su posición inicial y la almacenamos como un double en la variable “inicio”; en ese momento empieza la búsqueda de contenedores.
  
  
• Buscar contenedor.
  El robot comienza a girar en sentido antihorario y a buscar con el sensor de ultrasonidos posibles contenedores. Va tomando medidas con el sensor de ultrasonidos, de 5 en 5, como en la fase inicial.
  En el momento en el que se obtienen 3 medidas válidas de estas 5 (se detecta 3 veces seguidas un objeto entre 0 y 90 cm de distancia), la variable “detectado” pasa a tomar el valor “true” y el robot pasa al estado de detección de contenedor.
  Nos quedaremos en el estado de búsqueda de contenedor mientras “detectado” sea “false".
  
  
• Detectando contenedor.
  En cuanto “detectado” cambia su valor a “true”, se lee con la brújula el ángulo inicial de ese contenedor y se almacena como un entero en la variable “di”.
  El robot sigue tomando lecturas del sensor de ultrasonidos y, en cuanto una de ellas sea mayor a 90 o indique que no hay ningún objeto (-1), sabe que ha llegado al final del contenedor. En ese momento se para y mide con la brújula el ángulo final del contenedor (variable “df”).
  El robot calcula el ángulo en el que se encuentra el punto medio del contenedor (destDegrees) como:
  
  
  
  Hay que tener en cuenta el caso particular de que el contenedor esté en el norte, pues en esa situación su ángulo inicial será cercano a 0 y el ángulo final cercano a 360. Para poder seguir aplicando esta fórmula, habrá que sumar 360 al ángulo inicial “di”. Ahora “destDegrees” podrá ser menor o mayor de 360; si es mayor, tendremos que restarle 360 grados para obtener todas las posiciones en un rango de 0 a 360º.
  Con este ángulo medio ya calculado, el robot comprueba que no hay otro contenedor que ya haya sido detectado y se encuentre cercano a esta posición (permitiendo que exista una desviación de 15º). De esta forma, evitamos que haya dobles detecciones de un mismo contenedor. Si anteriormente había detectado un contenedor en el intervalo [destDegrees-15, destDegrees+15], pondrá la variable “añadido” a “true” y seguirá buscando nuevos contenedores. Si efectivamente es un contenedor nuevo, dejará “añadido” a “false” y pasará a la fase de comprobación de color.
  
  
• Comprobando color.
  Para poder comprobar el color de un contenedor, en primer lugar el robot se tiene que orientar hacia su punto medio. Por lo tanto, empieza a girar en sentido contrario hasta que mide con la brújula que se encuentra en una posición igual a “destDegrees”.
  Cuando ya se ha situado en la posición correcta, estima con el sensor de ultrasonidos a qué distancia se encuentra el contenedor y la almacena en la variable “dist”. El robot avanzará una distancia “dist”-5 cm, pues para tomar una lectura con el sensor de color necesita estar muy cerca del contenedor. Debido a la poca precisión del sensor de ultrasonidos, la estimación de esta distancia no es muy buena y, en algunas ocasiones, el robot se acerca demasiado al contenedor.
  Cuando el robot alcanza el contenedor, comprueba su color con el sensor de color. En ese momento se asegura de que anteriormente no había detectado un contenedor de ese mismo color (nunca deberíamos llegar a detectar dos veces un contenedor del mismo color, pues en el estado “detectando contenedor” ya hacíamos una comprobación para evitar dobles detecciones). Si ya teníamos un contenedor de ese color, se pone a “true” la variable “contenido” y se sigue buscando nuevos contenedores. Si es un color que todavía no tenemos y se encuentra dentro de la gama de colores válidos (rojo, amarillo, azul y verde), pasamos al estado de “añadir contenedor".
  
  
• Añadir contenedor.
  Tras comprobar que, efectivamente, hemos encontrado un contenedor que todavía no teníamos almacenado, creamos un nuevo objeto de la clase Container, que se almacena en un ArrayList de objetos Container.
  Al constructor le pasamos como parámetros: la posición del contenedor respecto al polo Norte magnético, la distancia en línea recta al punto central donde se coloca el robot en el escenario y su color.
  El robot retrocede la misma distancia que había avanzado para detectar el color del contenedor. Si tiene almacenados menos de 4 contenedores, seguirá buscando. Si ya tiene 4 contenedores, se termina la detección.
  
  
• Fin de la detección.
  La detección se termina cuando en el ArrayList de contenedores ya hay 4 elementos y todos ellos son de distinto color: rojo, amarillo, azul y verde. La última acción del robot es seguir girando hasta que se encuentra en la posición en la que había empezado la búsqueda de contenedores, almacenada en la variable “inicio”.
  El robot se parará y se quedará esperando a que accionemos el botón de “Procesar” desde la aplicación Android para poder proceder a la clasificación de los residuos.