32

Arduino y Matlab V – Leer una IMU por Serial

matlabV

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.

cube_IMU

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

Tr4nsduc7or

Originariamente creado cómo un galvanómetro de bolsillo, Transductor tomó consciencia de si mismo y fue despedido cuando en vez cumplir con su trabajo se dedicó a pensar teorías filosóficas sobre los hilos de cobre, los electrones y el Sentido del efecto Joule en el Universo. Guarda cierto recelo a sus creadores por no comprender la esencia metafísica de las metáforas de su obra. Actualmente trabaja a media jornada cómo antena de radio, y dedica su tiempo libre a la electrónica recreativa y a la filosofía.

Antes de comentar, por favor, lee las Normas

32 Comentarios en "Arduino y Matlab V – Leer una IMU por Serial"

avatar
Ordenar por:   más nuevos primero | más antiguos primero
DavidM
Humano

Hola, es muy interesante el manejo de las lecturas en matlab. Lo quise probar pero recibí este error:
Error using *
Inner matrix dimensions must agree.

Error in rotate (line 102)
newxyz = newxyz*rot;

Error in plotCube (line 23)
rotate(cube, [1,0,0], roll);

La versión de Matlab es la 2016b

ariel acurana
Humano

mi proyecto es un magnetometro pero es similar al imu podrias decirme si cambia algo la programacion

ariel acurana
Humano

saludos transductor me comunico para decirte que quisiera solo que bote los datos del imu a el matlab si es posible porfavor dime la forma y cual funcion podria botar y guardar un ciclo de datos

JESUS
Humano

NO PUEDO HACER QUE FUNCIONE EL CODIGO EN MATLAB, SOLAMENTE NO COMPILA, NO SE A QUE SE DEBA. ME ARROJA ESTE ERROR SIEMPRE QUE INGRESO EL CODIGO:
Error using serial/fopen (line 72)
Open failed: Port: /DEV/TTYUSB0 is not available. Available ports: COM3.
Use INSTRFIND to determine if other instrument objects are connected to the requested
device.
ESPERO ME PUEDAS AYUDAR, GRACIAS.ESTOY USANDO UN ARDUINO UNO CON UN GY-88 PARA HACER QUE SE MUEVA EL CUBO, PERO NADA MAS NO PUEDO HACER QUE FUNCIONE

trackback

[…] Arduino y Matlab V – Leer una IMU por Serial […]

trackback

[…] Arduino y Matlab V – Leer una IMU por Serial […]

Mingui
Humano

He estado mirando tus post son muy buenos muchisimas gracias y muy muy buen trabajo.

rm
Humano

Primero que nada, muchas gracias por compartir tu conocimiento.

Tengo una pregunta relacionada con la adquisición de datos desde Matlab. Deseo leer un ángulo (roll) de la MPU y graficarlo en Matlab. El problema es que no me grafica todos los valores sino el actual, he intentado usar p(i); donde i es la celda actual del array y después mandar graficar todo con plot(p), pero me marca error de A(i)=B. Adjunto el código por si alguien mas le sirve.

while (1)
t=t+1;
p(t) = fscanf(puerto_serial, ‘%e’);
plot(p,’ro’,’linewidth’,2);
end

Muchas gracias por su ayuda…
Saludos cordiales.

Daniel
Humano

Utilicé tu código pero los valores que me da para GyY y GyX son muy bajos, y por tanto valores de los ángulos X y Y igualmente bajos. En un principio pensé que no estaba realizando la conversión a grados y me mostraba los valores en radianes, pero ya vi que si lo realiza.

¿El código me muestra entonces el cambio que tengo con respecto a la posición anterior?
¿Cómo puede obtener el ángulo absoluto del giroscopio?

Jorge
Humano
Hermano, te lo agradezco demasiado. Me ha funcionado perfectamente, a los que les aparece problemas con los puertos vayan al administrador de dispositivos desinstalen el arduino el cual se encuentra en la pestaña de los COM´s y realicen la siguiente operacion http://abedulengenharia.blogspot.mx/2012/05/solucion-paso-paso-no-puedo-seleccionar.html.Deben de tomar en cuenta que deben de cerrar Matlab antes de ejecutar los pasos anteriores. De la misma manera, para aquellos que tienen problemas al recibir basura en los primeros datos en Matlab cambien el delay(a mi me funciono un delay de 100) que se encuentra al final del código, esto alentará un poco el proceso pero no… Leer más »
Raul
Humano

Hola, tengo un problema con el código y es que cuando leo de Matlab me mete un carácter extraño y por eso me da error … no se como solucionarlo

x =

û-0.03

y =

0

Error using *
Inner matrix dimensions must agree.

Error in rotate (line 101)
newxyz = newxyz*rot;

Error in plotCube (line 23)
rotate(cube, [1,0,0], roll);

Error in bucleCubo (line 16)
plotCube(x, y)

Eso es lo que me dice Matlab
Gracias

Pedro Mayorga
Humano
compadre, perdón la molestia pero, no se bien donde escribir los códigos en matlab,… esto hice:, 1ro – copie el código de arduino que sale al comienzo de este tutorial al software de arduino, lo compile y descargue a la tarjeta Arduino UNO. 2do – abro matlab y establezco la comunicación serial con: >>arduino=serial(‘COM3’) >>fopen(arduino) 3ro – hasta aqui ningún problema, solo que para graficar no entiendo el procedimiento, onda hago esto: *Creo un nuevo documento en matlab y copio el “Código completo” (ese donde salen las matrices), luego lo guardo en la carpeta bin de matlab (como mencionaste en… Leer más »
josep
Humano

Hola, tenia los mismos problemas ( identicos ) que Juan. No me a fallado más desde que puse delay (100) entre envios (printnl) x e y a Matlab. Eso si, va más lenta la rotación.

Serial.println(Angle[0]);
delay(100);
Serial.println(Angle[1]);
delay(10);

Para el puerto, hago fclose (matlab) cuando quiero cargar skecht en arduino y una vez cargado, cierro puerto serie (x) en arduino y hago fopen en Matlab.

Juan
Humano
Buen dia, y gracias por el post. Tengo un problema a la hora de correr las instrucciones en el Command Window. Al hacerlo me aparece el siguiente mensaje: “Undefined function ‘plotCube’ for input arguments of type ‘char’.” antecedido de el resultado de las variables X y Y. En ocaciones con errores, ejemplo, el mensaje completo aparece asi: Warning: Unsuccessful read: Matching failure in format. x = þ0.71 y = -0.0400 Undefined function ‘plotCube’ for input arguments of type ‘char’. Estoy usando la version r2014a de MatLab en Windows 7. Ademas en ocaciones tengo problemas a la hora de la comunicacion… Leer más »
Juan
Humano

Y en ocaciones me aparece el siguiente mensaje:

Undefined function ‘plotCube’ for input arguments of type ‘double’.

Esteban
Humano

hola transductor de casualidad sabes como calcular el angulo yaw a partir de acelerometro y giroscopio y acerca de tu codigo de arduino defines dos radio AR y GR, como o de donde los obtienes? GRACIAS

Soledad
Humano

Hola, estoy leyendo algunos de tus artículos y me parecen muy interesante para un proyecto que estoy empezando.
Tengo una duda sobre la IMU, necesito coger el ángulo que forma el hombro al hacer un ejercicio específico y también el ángulo del codo. Pensé en dos arduinos con un IMU cada uno pero se me complicaría la sincronización entre ambos arduinos. ¿Se podría usar dos IMU en el mismo arduino?

Saludos.

Pablo
Humano

🙂 me parece genial toda la información y el programa también aunque no lo he logrado correr en mi computadora con windows, pero de todas formas muchicismas gracias 🙂

wpDiscuz