En este momento podemos proceder a la programación de movimientos con nuestro sensor IMU de movimiento.

Para ello necesitaremos realizar un montaje que contenga dos servomotores, el sensor y un dispositivo Bluetooth, para facilitar el modo de pruebas y comprobar que nuestro robot funciona adecuadamente.

En caso de no disponer de un módulo Bluetooth en el siguiente post indico la manera de programar eventos en el tiempo para este modo de pruebas.

En caso de no disponer de un montaje, podéis imprimir cualquier modelo de Thingiverse. Pero ya puestos, os recomiendo desargaros a Clumsee y así extenderemos su utilidad en lecciones posteriores.

Con cualquiera de estos dos montajes se puede hacer funcionar nuestro pequeño robot mediante el modelo de orientación.

En este tutorial vamos a obtener la orientación del sensor como ya explicamos en la lección anterior y a través de un sistema de comunicación informar a nuestro robot cuál es la orientación que queramos a la que se dirija.

Para ello vamos a desarrollar 2 apartados

Control de orientación
- Cambio del ángulo de orientación mediante Bluetooth
  - U – 180º
  - L – 90º
  - R – 270º
  - B – 0º (360º)
- Rango de elección de eventos
Funciones de movimiento
- forward()
- backward()
- right()
- left()
- stop()

CONTROL DE MOVIMIENTO CON SENSOR IMU

Antes de desarrollar ninguna programación, vamos a estudiar durante un momento cuáles van a ser algunos de los objetivos que ha de cumplir nuestro robot para que funcione a la perfección.

Robustez frente a variaciones de orientación
Rango circunferencial
Evitar bloqueos de programa – Monitor Debug
Determinismo de estados
Inercia del movimiento

Robustez frente a variaciones de orientación

Nuestro principal objetivo es que nuestro robot tienda a dirigirse hacia una dirección concreta. determinada por un valor de entrada que ha de ser comparado con el valor que obtiene del sensor de orientación, tal y como explicamos en lecciones anteriores.

Esto quiere decir que habremos creado un sistema realimentado en el que nosotros definimos un Input como la señal de entrada y que en función de la salida u Output, (la información obtenida con el sensor) nuestro robot determinará hacia que lado habrá de moverse.

Es decir, en cada iteración del bucle loop() de nuestro programa, obtendremos la información del sensor de orientación, y por otra parte, el ángulo al que queremos llegar. Si comparamos estas dos variables; decidiremos entonces hacia donde hemos de girar o movernos.

El fallo de este modelo de control es que solamente podremos movernos hacia la izquierda o hacia la derecha; pero queremos que cuando nuestro sensor mida el mismo valor que el ángulo que nosotros definimos vaya en linea recta.

El problema es que es muy difícil que nuestro sensor y el ángulo que definamos sea exactamente igual en todo momento, ya que la mas pequeña variación del ángulo medido provocará que deje de ser igual un número elevado de veces.

Es por ello, que vamos a desarrollar una solución extendida; que he denominado como rango angular.
Es tan sencillo como establecer un limite umbral para el ángulo para el cual determinaremos si queremos ir recto o queremos girar hacia uno u otro lado.

Este control ha de ejecutarse en cada momento que nuestro robot mida el valor de orientación para poder responder a su desvio. De esta manera, el diagrama de flujo queda determinado de la siguiente manera.

Rango circunferencial

Ahora podríamos realizar un primer programa para desarrollar este control que hemos definido. Pero para no perder el tiempo os cuento lo que ocurrirá.

Si nuestro objetivo es definir solamente 4 direcciones de movimiento (Norte, Sur, Este y Oeste); el programa nos funcionará en 3 de las 4 opciones. En el caso de definir numericamente el ángulo de giro, el programa fallará en un intervalo muy concreto.

Si nos fijamos en la imagen superior, el valor para el que he elegido el Norte es el ángulo de 180º; y no es un valor elegido al azar.

El problema reside en que el valor para un ángulo de 0 es el mismo que 360. Esto quiere decir que hay un salto de 360 grados que cae en picado si queremos definir límites a izquierda y a derecha.

Inevitablemente vamos a tener una discontinuidad en la función leida del sensor en las cercanía del 0.

En el caso de establecer el ángulo objetivo como el 0, dispondríamos los sisguientes límites; tal y como hemos definido antes con un umbral de 25; entre 25 y -25.

En el caso de obtener un ángulo del sensor de 345 por ejemplo. La sentencia no proporcionará el valor correcto para su giro.

Al igual con el valor de 360; dispondremos de un valor umbral de 335 que se ejecuta correctamente, pero el segundo umbral se obtiene de 385.

Por lo que tampoco se ejecuta correctamente. Y así con todos los intervalos de ángulo que crucen el salto entre 0 y 360 grados que se va a evaluar del sensor de orientación.

Es por ello que hay que manipular o filtrar estos números para obtener la acción deseada.

En un principio se podría pensar que lo que hay que hacer es desarrollar un algoritmo que salve ese salto de 360 grados en una circunferencia.

Pero si lo pensamos detenidamente, nuestro robot, no sabe interpretar que el Norte es un 0 o son 180 grados. Es decir, no dispone de una referencia para que estos intervalos tengan sentido. Es por ello, que lo que vamos a modificar, es la referencia de nuestro robot para hacer corresponder el valor de 180º con el ángulo hacia el que nosotros queremos girar. Así que lo que tenemos que hacer es girar la circunferencia que nuestro robot toma como referencia.

Para ello, vamos a aplicar unas matemáticas muy sencillas. Vamos a utilizar una función a trozos en la que vamos a redefinir unos valores para la recta que define una circunferencia a la que vamos a aumentar un valor y que se resuelva dentro de los límites de los 360 grados.

Es decir, tenemos que conseguir trasladar la recta de valores de una circunferencia de 0 a 360 grados hacia otra recta con un desfase de tantos grados como queramos establecer esa orientación en el punto de equilibrio de 180º.

En la siguiente imagen vamos a crear una nueva función que gire 180 grados la referencia circunferencial de nuestro robot para reconocer qué efecto tiene.

Latex function

(1) $\begin{equation*} f(x) = \left\{ \begin{array}{ll} x+c & \mathrm{si\ }0 \le x \le 360-c \\ x - (360-c) & \mathrm{si\ } 360-c \le x \le 360 \\ \end{array} \right. \end{equation*}$

Para ello utilizaremos una nueva variable, denominada new_yaw y será la ue utilizaremos con respecto a la variable yaw que obtenemos del sensor. Esta sección del programa quedará de la siguiente manera.

Determinismo de estados

Como hemos dicho anteriormente, vamos a evitar los pequeños bloqueos que se obtienen de repetir algunas funciones de forma innecesaria. En lugar de activar los servomotores en cada iteración, solo lo haremos cuando exista una modificación en su movimiento, detectado por el sensor. Para ello, requerimos de una variable que determine el estado actual con respecto a un estado pasado; y en el momento que dejen de ser iguales, ejecutar la acción e igualarlas.

Evitar bloqueos de programa – Monitor Debug

Por último, se puede añadir una pequeña función en la que integremos un modo de visualizar el resultado anterior de angulos y observar como evoluciona nuestro programa con la función de rango aplicada y saber que lo estamos aplicando correctamente.

Primer ejercicio con ArduBlockly

A través de la página de ArduBlockly podemos realizar el siguiente código.

Ahora solo deberemos instalar una aplicación que nos envíe los caracteres U, D, L, R por Bluetooth para determinar la dirección del ángulo de giro de nuestro robot de forma remota.

Robopad es una aplicación que ofrece estas posibilidad.

Esta solución nos será válida para todos los ángulos de giro que queramos disponer. En principio nuestro programa quedará de la siguiente manera.

#include <MadgwickAHRS.h>
#include <IMU_MPU6050.h>
#include <SoftwareSerial.h>

#define DEBUG true

float newyaw;
float roll;
float pitch;
float yaw;
int reference;
int angle_threshold;
int angle;
String last_move;
String actual_move;
int bound1;
int bound2;
char serialData;
long microsPerReading;

Madgwick filter;
MPU6050 imu;
SoftwareSerial comm(2,3);

void updateIMU(float *roll, float *pitch, float *yaw){

  if (imu.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01){    
    imu.readAccelData(imu.accelCount);
    imu.getAres();

    imu.ax = (float)imu.accelCount[0]*imu.aRes;
    imu.ay = (float)imu.accelCount[1]*imu.aRes;
    imu.az = (float)imu.accelCount[2]*imu.aRes;
    imu.readGyroData(imu.gyroCount);
    imu.getGres();

    imu.gx = (float)imu.gyroCount[0]*imu.gRes;
    imu.gy = (float)imu.gyroCount[1]*imu.gRes;
    imu.gz = (float)imu.gyroCount[2]*imu.gRes;
  }

  imu.updateTime();
  imu.delt_t = millis() - imu.count;
  if (imu.delt_t > microsPerReading){
    filter.updateIMU(imu.gx, imu.gy, imu.gz, imu.ax, imu.ay, imu.az);
    *roll = filter.getRoll();
    *pitch = filter.getPitch();
    *yaw = filter.getYaw();
    imu.count = millis();
  }
}

void setup() {
  filter.begin(25);
  Wire.begin();
  microsPerReading = 1000 / 25;
  imu.calibrateMPU6050(imu.gyroBias, imu.accelBias);
  imu.initMPU6050();
  Serial.begin(38400);
  comm.begin(38400);
  
  newyaw = (float)(0);
  roll = (int)(0);
  pitch = (int)(0);
  yaw = (int)(0);
  reference = (int)(0);
  angle_threshold = (int)(25);
  angle = (int)(180);
  last_move = (String)("FORWARD");
  actual_move = (String)("FORWARD");

}

void loop() {

  bound1 = (int)(angle - angle_threshold);
  bound2 = (int)(angle + angle_threshold);

  updateIMU(&roll, &pitch, &yaw);

  while (comm.available()) {
    serialData = (char)((comm.read()));
    if (serialData == ('U')) {
      reference = 0;
    } else if (serialData == ('D')) {
      reference = 180;
    } else if (serialData == ('R')) {
      reference = 90;
    } else if (serialData == ('L')) {
      reference = 270;
    }
  }

  //Change reference on yaw circumference
  if (yaw < 360 && yaw > (360 - reference)) {
    newyaw = yaw - (360 + reference);
  } else {
    newyaw = yaw + reference;
  }

  //Range values for movement in IMU direction
  if (newyaw > bound1 && newyaw < bound2) { 
    actual_move = "MOVE FORWARD"; 
  } else if (newyaw > bound2) {
    actual_move = "MOVE RIGHT";
  } else if (newyaw < bound1) { 
    actual_move = "MOVE LEFT"; 
   } 

   //Robot state change once 

   if (last_move != actual_move) { 
      Serial.println(actual_move); 
      last_move = actual_move; 
   } 

   //Debug data from IMU Sensor 
   if (DEBUG){ 
      IMUDebug(); 
   } 
} 

long last_time = 0; 

void IMUDebug(){ 
   if ( millis() - last_time > 500){
     Serial.print("Orientation ");
     Serial.print(yaw);
     Serial.print(" new: ");
     Serial.println(newyaw);
    
     /*Serial.print(" ");
     Serial.print(pitch);
     Serial.print(" ");
     Serial.println(roll);*/
     last_time = millis();
  }
 };

Inercia del movimiento

Un aspecto que es muy crítico frente al diagrama anterior, es que los servomotores, pueden girar demasiado rápido en la entrada al intervalo en el que han de cambiar de estado de uno de los giros, al movimiento hacia delante.

O en el caso de estar parados pueden dar algun movimiento indeseado cuando realizamos a velocidad 0 con la función de servo.write(0).

Es por ello que vamos a elaborar unas funciones más elaboradas para su movimiento en el que introduzcamos un detach, para que no haya señales residuales que provoque un movimiento sobre el robot que pueda perjudicar erroneamente a la orientación.

Este modo de control nos ayudará a desconectar los servomotores en el momento que haya llegado al ángulo que requerimos cuando pare.

Ahora solo nos faltaría integrar los movimientos dentro del programa anterior.

IMU Control + Movement