Arduino y Matlab V – Leer una IMU por Serial

Saludos, humano. Al habla Transductor. Hoy, siguiendo con la série de tutoriales de Arduino y Matlab, voy a enseñarte cómo visualizar las lecturas de una IMU en Matlab, concretamente una MPU-6050.

El resultado final será un cubo color rojizo que vas a poder girar con la IMU, un pasatiempo muy entretenido si la otra opción es, por ejemplo, mirar fijamente a un muro o hablarle de Filosofía Hegeliana a un cactus.

El cubo. También conocido por: hexaedro, hexaedro regular, dado, bloque…

Antes de seguir éste tutorial, deberías mirarte Arduino y Matlab IV – Leer Comandos por Serial, para entender cómo enviar comandos por Serial a Matlab. También, si quieres entender el funcionamiento de mi código y no limitarte a copiarlo, recomiendo que leas el tutorial que redacté sobre el MPU-6050. En él explico cómo funciona la IMU y cómo conectarla y de qué forma debes configurarla con Arduino para aplicar un Filtro Complementario y obtener lecturas precisas, puesto que no voy a entrar en detalles.

Dicho esto, es hora de programar.

Material necesario

• Arduino. Esta vez utilizo la versión Nano, pero cualquiera vale.
• MPU-6050 conectada a Arduino. Insisto, si no sabes cómo funciona y no quieres fundirla echa un vistazo a mi tutorial.
• Ordenador con Matlab instalado
• Cable USB de tipo impresora

El código para Arduino

El código para Arduino será una variante del que escriví para el tutorial del MPU-6050. Simplemente he cambiado la línea que envía por Serial los outputs, a fin de que sea más fácil trabajar con los valores en Matlab:

#include <Wire.h>

//Direccion I2C de la IMU
#define MPU 0x68

//Ratios de conversion
#define A_R 16384.0
#define G_R 131.0

//Conversion de radianes a grados 180/PI
#define RAD_A_DEG = 57.295779

//MPU-6050 da los valores en enteros de 16 bits
//Valores sin refinar
int16_t AcX, AcY, AcZ, GyX, GyY;

//Angulos
float Acc[2];
float Gy[2];
float Angle[2];

void setup()
{
    Wire.begin();
    Wire.beginTransmission(MPU);
    Wire.write(0x6B);
    Wire.write(0);
    Wire.endTransmission(true);
    Serial.begin(9600);
}

void loop()
{
        //Leer los valores del Acelerometro de la IMU
    Wire.beginTransmission(MPU);
    Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
    Wire.endTransmission(false);
    Wire.requestFrom(MPU,6,true); //A partir del 0x3B, se piden 6 registros
    AcX=Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
    AcY=Wire.read()<<8|Wire.read();
    AcZ=Wire.read()<<8|Wire.read();

        //Se calculan los angulos Y, X respectivamente.
    Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
    Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;

        //Leer los valores del Giroscopio
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU,4,true); //A diferencia del Acelerometro, solo se piden 4 registros
    GyX=Wire.read()<<8|Wire.read();
    GyY=Wire.read()<<8|Wire.read();

        //Calculo del angulo del Giroscopio
    Gy[0] = GyX/G_R;
    Gy[1] = GyY/G_R;

        //Serial.print(Gy[0]); Serial.print("||"); Serial.print(Gy[1]); Serial.print("n");

        //Aplicar el Filtro Complementario
    Angle[0] = 0.98 *(Angle[0]+Gy[0]*0.010) + 0.02*Acc[0];
    Angle[1] = 0.98 *(Angle[1]+Gy[1]*0.010) + 0.02*Acc[1];

        //Mostrar los valores por consola
    //Serial.print("Angle X: "); Serial.print(Angle[0]); Serial.print("n");
    //Serial.print("Angle Y: "); Serial.print(Angle[1]); Serial.print("n------------n");
        Serial.println(Angle[0]); Serial.println(Angle[1]);

    delay(10); //Nuestra dt sera, pues, 0.010, que es el intervalo de tiempo en cada medida
}

Compila y carga el programa. Una vez hecho, es hora de abrir Matlab.

Crear una función: plotCube()

En vez de escribir un programa para visualizar cómo cambian los números que envía Arduino por Serial, hoy me siento inspirado para hacer algo mejor. El siguiente paso será crear una función que dibuje un cubo y lo vaya rotando a medida que cambie el ángulo de la IMU. Y la función se llamará plotCube().

Si tienes algo de experiencia en Matlab, sabrás que no es posible declarar las funciones en la Ventana de Comandos. Deben escribirse en el Editor de Funciones. Hay que pulsar New -> Function, y aparecerá otra ventana similar a un editor de texto dónde programar la función.

plotCube() recibirá dos parámetros llamados roll y pitch, que van a corresponderse a los ángulos X e Y respectivamente. La función también dibujará un cubo y lo pintará de color rojo. Después lo girará en función de los parámetros de entrada.

Empezamos por inicializar la función y sus argumentos:

function plotCube(roll, pitch)

Para crear el cubo se necesitan dos arrays. El primero, llamado vertex_matrix, almacena las coordenadas XYZ de todos los vértices del cubo, mientras que faces_matrix almacenará las caras.

Estos dos arrays son de manual.

vertex_matrix = [0 0 0
1 0 0
1 1 0
0 1 0
0 0 1
1 0 1
1 1 1
0 1 1]-0.5;

faces_matrix = [1 2 6 5
2 3 7 6
3 4 8 7
4 1 5 8
1 2 3 4
5 6 7 8];

El siguiente paso es mostrar el cubo en pantalla.

axis([-1 1 -1 1 -1 1]);
axis equal off;
cube = patch('Vertices',vertex_matrix,'Faces',faces_matrix,'FaceColor', 'red');

Los parámetros de axis sirven para establecer cómo va a mostrarse el cubo. La lista:

axis([-1 1 -1 1 -1 1]);

Sirve para limitar el valor máximo y mínimo de los ejes X, Y, Z. Sin ella, podría haber partes del cubo que quedarían fuera de la cámara.

En cuánto a los parámetros

axis equal off;

equal sirve para evitar que Matlab deforme la imagen para ajustarla a la ventana. off sirve para esconder la rejilla con las medidas de los ejes.

Por último, el comando patch creará una figura 3d llamada cube a partir del vertex_matrix, el faces_matrix y de color rojo.

Ahora rotamos a cube:

rotate(cube, [1,0,0], roll);
rotate(cube,[0,1,0], pitch);
view(0,0);

end

La instrucción rotate recibe tres parámetros. El primero es el objeto que debe rotarse, en este caso cube. El segundo es la dirección de rotación [x,y,z]. [1,0,0] se corresponde al eje X. El último parámetro es un valor numérico, correspondiente a los grados que debe girar.

view() recibe dos parámetros, azimuth y elevación. view(0,0) coloca la cámara totalmente horizontal. Y lógicamente queremos la cámara horizontal para no distorsionar la perspectiva.

Y end cerrará la función.

El código completo será:

function plotCube(roll, pitch)

 vertex_matrix = [0 0 0
1 0 0
1 1 0
0 1 0
0 0 1
1 0 1
1 1 1
0 1 1]-0.5;

faces_matrix = [1 2 6 5
2 3 7 6
3 4 8 7
4 1 5 8
1 2 3 4
5 6 7 8];

subplot(1,3,1)
axis([-1 1 -1 1 -1 1]);
axis equal off;
cube = patch('Vertices',vertex_matrix,'Faces',faces_matrix,'FaceColor', 'red');
rotate(cube, [1,0,0], roll);
rotate(cube,[0,1,0], pitch);
view(0,0);

end

Guarda esta función en el directorio bin de Matlab y sal del editor.

ACTUALIZACIÓN: Recordaos de guardar la función con el nombre plotCube.m . De lo contrario habrá problemas.

Instrucciones para la Ventana de Comandos:

En la Command Window hay que crear un bucle infinito que vaya leyendo y mostrando las lecturas de la IMU.

IMPORTANTE: para detener el bucle, pulsa Ctrl+C. Si no, vas a tener cubo para rato.

arduino = serial('/dev/ttyUSB0')

fopen(arduino)

while 1
x = fscanf(arduino, '%e')
y = fscanf(arduino, '%e')
plotCube(x, y)
drawnow
clf
end

No debería tener mucho secreto. while 1 es un bucle infinito, fscanf lee un fichero, en éste caso arduino, y la notación ‘%e’ significa que debe leer un float.

plotCube(x,y) llama la función que he escrito antes.

El comando drawnow le dice a matlab que dibuje el gráfico ahora. Sin él, esperaría a terminar el bucle y mostraría la posición final del cubo.

clf borra el dibujo a cada iteración.Sin el comando de borrar, a cada iteración Matlab dibujaría un nuevo cubo encima del anterior.

Si ahora pulsas enter, verás que aparece una pantalla con un cubo rojizo en el centro. Si mueves la IMU, el cubo girará, pero sólo en los ejes X, Y. Recuerda que desactivamos la rotación del eje Z (llamada yaw) en el tutorial del MPU-6050.

¿Y ahora qué?

“Transductor… ¡Se mueve, pero va con retraso!”. Es normal. En un primer momento, el movimiento va con algo de retraso. Espera un momento y si no funciona, vuelve a iniciar el bucle.

“Transductor… ¡Esto no se mueve!”. No es normal. El código que he publicado aquí es exactamente el que yo he utilizado. Revisa tu programa, línea por línea. También vigila el sketch de Arduino. Y por supuesto, mira las conexiones de la IMU.

“Transductor… me gira, ¡Pero va al revés!” Es posible que te el cubo te gire bien, pero al revés de como debería. Si es tu caso, abre el editor de funciones y cambia el signo de las variables roll e/o yaw.

“Transductor… ¿Sería posible crear otros dos cubos, uno para las lecturas del acelerómetro y otro para las del giroscopio?” En efecto. La idea es la misma. Deberás crear dos cubos más en la función, llamados cube2 y cube3. Deberás utilizar el comando subplot() para mostrar los tres cubos en un solo gráfico. Además necesitarás más argumentos, dos para el acelerómetro y dos para el giroscopio. Es decir:

subplot(1,3,x)
axis([-1 1 -1 1 -1 1]);
axis equal off;
cubeX = patch('Vertices',vertex_matrix,'Faces',faces_matrix,'FaceColor', 'red');
rotate(cube, [1,0,0], argumento_x);
rotate(cube,[0,1,0], argumento_y);
view(0,0);

Sustituyendo la x de subplot() por el número de cubo, y cubeX por cube2 o cube3. argumento_x y argumento_y serán los argumentos x, y del giroscopio/acelerómetro.

“Transductor… ¿puedo escribir mis dudas en los comentarios?” ¡Por supuesto, lector! Yo o alguno de mis compañeros te responderemos lo más rápido posible.

Final de línea.

<-Volver a la Parte IV