Software básico del robot (Parte III)

En esta entrada completamos la descripción del software del robot: veremos qué comandos puede interpretar, cómo se divide su inteligencia en comportamientos, qué acciones ejecuta al cumplirse la condición de inicio de cada uno de ellos y cómo se establecen sus prioridades.

Actualmente hemos implementado 6 comportamientos distintos que, ordenados de mayor a menor prioridad son: Stop(), MotorSpeed(), Monitoring(), Shoot(), Mapping() y ProcessData(). Como ya sabemos, en esta arquitectura subsumption podríamos añadir o eliminar behaviors en cualquier momento sin afectar al resto.

Desde una aplicación Android muy sencilla en la que disponemos de botones que nos permiten elegir la acción que debe realizar el robot, enviamos por Bluetooth unos bytes de comando. Como tenemos 6 behaviours, necesitamos 3 bits para informar al robot de qué comportamiento debe tomar el control: serán los 3 bits más significativos del primer byte que se envía (c2, c1 y c0).

Con este protocolo de comunicación, el método takeControl() de todos los behaviours desplazará 5 posiciones a la derecha el byte recibido y comprobará si c2, c1 y c0 coinciden con el código de comando que se le ha asignado a ese comportamiento en concreto.

A continuación pasamos a explicar con detalle qué realiza el método action() en cada uno de los behaviours, empezando por los que tienen una mayor prioridad:

1. Stop() (c2 c1 c0 = 000): Detiene el robot en caso de emergencia, parando los 3 motores que tiene y vaciando la pila de comandos pendientes.
   
   
2. MotorSpeed() (c2 c1 c0 = 001): Se encarga de controlar la velocidad de los motores. En este caso no nos basta con mandar por Bluetooth un único byte que determine qué comando se ha de ejecutar, sino que necesitamos 2 bytes adicionales: el segundo indica la velocidad del motor izquierdo (motor A) y el tercero la del motor derecho (motor B). Como en Java tenemos bytes con signo, en complemento a 2, el valor de velocidad que enviamos por Bluetooth estará comprendido entre -128 y 127. El signo indica si el motor tiene que girar en sentido de avance o retroceso. El valor absoluto indicará el módulo de la velocidad, multiplicándolo previamente por un factor: la velocidad del robot puede variar entre 0 y 900, pero sólo dejaremos que alcance 700 para tener un margen de seguridad; por lo tanto, para calcular la velocidad final tendremos que multiplicar por un factor de 700/128= 5,46.
   
   
3. Monitoring() (c2 c1 c0 = 110): Nos permite monitorizar el estado de los sensores. Comprueba la distancia a la que se encuentra un objeto del sensor de ultrasonidos, de qué color es la siguiente pelota a disparar y de qué color es el contenedor que tiene delante (en caso de que haya encontrado alguno). Estos datos se mandan por un stream de salida, por Bluetooth.
   
   
4. Shoot() (c2 c1 c0 = 011): Simplemente dispara una pelota, haciendo que el motor B gire 360º a su máxima velocidad.
   
   
5. Mapping() (c2 c1 c0 = 010): Este comportamiento ha sido modificado varias veces para buscar el mejor algoritmo de clasificación de residuos, por lo que explicaremos cómo funciona en su última actualización. Cuando toma el control, hace que el robot dé unas vueltas de reconocimiento para detectar los contenedores que hay en su entorno y poder clasificar los residuos en su correspondiente contenedor. Aquí podemos ver su diagrama de estados actual, con una explicación de qué ocurre en cada estado:
   
   
   
   
   
   
   - Detectando contenedor: Cuando empieza a detectar un objeto que se encuentra dentro del rango de distancias establecido, almacena en las variables temporales “disttemp” y “tachotemp” a qué distancia y a qué ángulo respecto del de partida lo ha detectado por primera vez. La variable booleana “detectado” se pondrá a “true”.
   
   -Comprobando color: Teniendo en cuenta el efecto provocado por la apertura del haz de ultrasonidos, sabemos que cada contenedor será detectado a lo largo de un determinado arco de circunferencia. Para poder determinar el punto medio del contenedor, necesitamos hacer la media entre el ángulo inicial de este sector circular (almacenado en “tachotemp”) y el ángulo en el que deja de detectar el contenedor (se obtiene como Motor.A.getTachoCount()). Al dejar de detectar un contenedor, hacemos que el robot rote en sentido contrario un ángulo “retroceder” (int retroceder = (Motor.A.getTachoCount() + 50 - tachotemp) / 2;). Con esto pretendemos que quede apuntando al punto medio del contenedor. Experimentalmente vimos que era necesario aplicar una corrección; por este motivo, aparece 50 sumando en la expresión de “retroceder”.
   
   Cuando ya tenemos al robot centrado respecto al contenedor, avanza hacia él una distancia “dist-10” y el sensor de color toma una lectura. Si el color del contenedor y el de la pelota del cargador coinciden, se pone la variable “dispara” a “true”.
   Finalmente, el robot retrocede la distancia que había avanzado para acercarse al contenedor.
   
   -Disparar pelota: Si, al volver al centro del escenario tras acercarse a un contenedor, el robot tiene “dispara==true”, accionará el motor B para disparar una pelota al contenedor.
   
   -Parada: Entramos en este estado cuando la variable “tachoTotal” se hace cero, lo cual quiere decir que el robot ya ha dado 4 vueltas completas en el escenario.
   
   
   -Buscar contenedor: El robot está programado para dar 4 vueltas de 360º en busca de contenedores a partir de un ángulo inicial de referencia. Los grados que quedan para terminar el mapeo se almacenan en la variable “tachoTotal”. Mientras el sensor de ultrasonidos no detecte ningún objeto a un radio menor que el establecido (actualmente, 70 cm), el robot seguirá rotando a una velocidad de motores constante (200). La variable donde se almacena la lectura del sensor de ultrasonidos es “dist”.
6. ProccesData() (c2 c1 c0 = 100): Ahora mismo este comportamiento no se está utilizando. Si conseguimos mejorar la detección de contenedores, lo usaremos para que, habiendo almacenado previamente la posición y el color de todos los contenedores del escenario, luego el robot vaya clasificando las pelotas.

También hay que señalar que, de momento, no hemos necesitado utilizar los métodos suppress() de ninguno de los behaviours.

No podemos olvidar que, para establecer esta arquitectura subsumption, necesitamos crear un árbitro que gestione los comportamientos. Esto se realiza en la clase main.java, con las siguientes líneas de código:

Behavior[] behaviours = { new ProcessData(), new Mapping(), new Shoot(), new Monitoring(), new MotorSpeed(), new Stop() }; // Crea los behaviours, ordenados de menor a mayor prioridad

ar = new Arbitrator(behaviours); // Inicializa el arbitro con los behaviours correspondientes

ar.start(); // Lanza el arbitro

Aunque con estos comportamientos descritos el robot ya es capaz de realizar todas las funciones básicas planteadas para el hito 1, estamos estudiando la posibilidad de incorporar un nuevo sensor I2C que nos permita mejorar la detección de contenedores. Si finalmente decidimos integrarlo, tendremos que modificar el software que aquí hemos explicado.