MPU-6050
DESCRIPCIÓN
Esta es la segunda parte del tutorial consiste en la implementación del IMU (unidad de medición inercial) utilizando un Arduino Uno para la lectura, procesamiento y cálculo de datos del módulo MPU-6050. Estos datos serán enviados mediante comunicación serial a una aplicación en Processing la cual hará una representación virtual en forma de cubo del MPU-6050 y una gráfica de tres ejes.
MATERIALES
– Acelerómetro Giroscopio MPU-6050
– Processing 2.0.3 (software)
– 1 Arduino Uno
– 1 Cables USB serial
DIAGRAMA y CONEXIÓN
PINES UTILIZADOS
INTRODUCCIÓN
Acelerómetro Giroscopio MPU-6050
DESCRIPCIÓN
IMU
Las Unidades de Medida Inercial están compuestas por un conjunto de sensores que miden aceleración, giro, y campo magnético. El objetivo de estos sensores en general es medir el movimiento en tres ejes para lograr localización o para estabilizar vehículos autónomos. Estas unidades en tienen múltiples aplicaciones en robótica, aeronaves, vehículos aéreos no tripulados, satélites, entre muchos otros.
En este tutorial únicamente se hace el uso del MPU-6050, primero se toman los valores del MPU-6050 con el Arduino, el mismo Arduino hará el cálculo de la aceleración y del giroscopio. Enviara los valores a serial y con la aplicación de Processing se generara el cubo, cabe mencionar que debes modificar la línea de Processing para elegir el COM correspondiente al Arduino Uno.
ESPECIFICACIONES
- Salida digital de 6 ejes.
- Giroscopio con sensibilidad de ±250, ±500, ±1000, y ±2000dps
- Acelerómetro con sensibilidad de ±2g, ±4g, ±8g y ±16g
- Algoritmos embebidos para calibración
- Sensor de temperatura digital
- Entrada digital de video FSYNC
- Interrupciones programables
- Voltaje de alimentación: 2.37 a 3.46V
- Voltaje lógico: 1.8V±5% o VDD
- 10000g tolerancia de aceleración máxima
Programa en C para Arduino Uno
Ya que el código es muy extenso, se utilizara el código del tutorial del Acelerometro. Para obtener el código completo favor descargarlo del siguiente link:
https://github.com/HeTpro/MPU6050_arduino_processing/tree/master
Variables:
[code language=»cpp»]
char str[512];
boolean firstSample = true;
float RwAcc[3]; //Proyección de vectores de fuerza gravitacional normalizada en ejes x,y,z medidos por el acelerómetro
float Gyro_ds[3]; //Lecturas de giroscopio
float RwGyro[3]; //Valores RAW obtenidos del ultimo estimado y movimiento del giroscopio
float Awz[2]; //Angulo entre la proyección de R en el plano XZ/YZ y eje Z (grados)
float RwEst[3];
int lastTime = 0;
int interval = 0;
float wGyro = 10.0;
void getAccelerometerData(int * result) {
int error;
accel_t_gyro_union accel_t_gyro;
error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
result[0] = (((int)accel_t_gyro.reg.x_accel_h) << 8) | accel_t_gyro.reg.x_accel_l;
result[1] = (((int)accel_t_gyro.reg.y_accel_h) << 8) | accel_t_gyro.reg.y_accel_l;
result[2] = (((int)accel_t_gyro.reg.z_accel_h) << 8) | accel_t_gyro.reg.z_accel_l;
}
[/code]
Lee los valores RAW, 14 bytes a la vez conteniendo aceleracion, temperatura y giroscopio. Con las configuraciones predeterminadas no hay filtro habilitado y los valores no son estables. Cada lectura de eje tiene una resolucion de 10 bits (2 bytes).
[code language=»cpp»]
void rawAccToG(int * raw, float * RwAcc) {
RwAcc[0] = ((float) raw[0]) / 16384.0;
RwAcc[1] = ((float) raw[1]) / 16384.0;
RwAcc[2] = ((float) raw[2]) / 16384.0;
}
[/code]
Conversión de valores RAW de acelerómetro a g’s, esto se logra dividiendo entre sensibilidad LSB según el rango de escala (ver hoja de datos, Register Map and Descripcion, Accelerometer Measurements)
[code language=»cpp»]
void getGyroscopeData(int * result)
{
int regAddress = 0x1B;
int temp, x, y, z;
int error;
accel_t_gyro_union accel_t_gyro;
error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
result[0] = (((int)accel_t_gyro.reg.x_gyro_h) << 8) | accel_t_gyro.reg.x_gyro_l;
result[1] = (((int)accel_t_gyro.reg.y_gyro_h) << 8) | accel_t_gyro.reg.y_gyro_l;
result[2] = (((int)accel_t_gyro.reg.z_gyro_h) << 8) | accel_t_gyro.reg.z_gyro_l;
}
[/code]
Lee los valores RAW, 14 bytes a la vez conteniendo aceleración, temperatura y giroscopio. Con las configuraciones predeterminadas, no hay filtro habilitado y los valores no son estables.
[code language=»cpp»]
void rawGyroToDegsec(int * raw, float * gyro_ds) {
gyro_ds[0] = ((float) raw[0]) / 131;
gyro_ds[1] = ((float) raw[1]) / 131;
gyro_ds[2] = ((float) raw[2]) / 131;
}
[/code]
Esta función hace la conversion de valores RAW de giroscopio a grados/seg. Dividiendo el valor RAW entre sensibilidad LSB segun el rango de escala (ver hoja de datos, Register Map and Descripcion, Gyroscope Measurements )
[code language=»cpp»]
void normalize3DVec(float * vector) {
float R;
R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
vector[0] /= R;
vector[1] /= R;
vector[2] /= R;
}
float squared(float x){
return x*x;
}
void getInclination() {
int w = 0;
float tmpf = 0.0;
int currentTime, signRzGyro;
currentTime = millis();
interval = currentTime – lastTime;
lastTime = currentTime;
if (firstSample) { // NaN es utilizado para esperar datos buenos del Arduino
for(w=0;w<=2;w++) {
RwEst[w] = RwAcc[w]; //inicializa las lecturas del acelerometro
}
}
else{
//evalua el vector RwGyro
if(abs(RwEst[2]) < 0.1) {
//Rz es demasiado pequeño y es utlizado como referencia para calcular Axz, Ayz sus fluctuaciones de error amplificaran llevándolo a malos resultados
//En este caso brincar el dato del giroscopio y utilizar el estimado anterior
for(w=0;w<=2;w++) {
RwGyro[w] = RwEst[w];
}
}
else {
//Tomar angulos entre la proyeccion de R en el plano ZX/ZY plane el eje Z,basado en el ultimo RwEst
for(w=0;w<=1;w++){
tmpf = Gyro_ds[w]; //toma valor de la tasa actual del grioscopio en grado/s
tmpf *= interval / 1000.0f; //tomar cambio de ángulo en grados
Awz[w] = atan2(RwEst[w],RwEst[2]) * 180 / PI; //Tomar ángulos y convertir a grados
Awz[w] += tmpf; //toma angulo actualizado segun el movimiento del giroscopio
}
// Hacer estimado de signo de RzGyro eso se sabe según el cuadrante está el ángulo Axz
//RzGyro es positivo si Axz está en un rango de -90 ..90 => cos(Awz) >= 0
signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
//calculo inverso de RwGyro a partir de ángulos Awz, para deducción de fórmulas ingresa a http://starlino.com/imu_guide.html
for(w=0;w<=1;w++){
RwGyro[0] = sin(Awz[0] * PI / 180);
RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
RwGyro[1] = sin(Awz[1] * PI / 180);
RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
}
RwGyro[2] = signRzGyro * sqrt(1 – squared(RwGyro[0]) – squared(RwGyro[1]));
}
//combina lectura de acelerómetro y giroscopio
for(w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro);
normalize3DVec(RwEst);
}
firstSample = false;
}
[/code]
CODIGO PROCESSING
Processing es un software y un lenguaje de programación basado en Java, con este lenguaje de programación se pueden hacer dibujos, graficas, etc., hasta crear programas tan complejos como juegos, interfaces, etc. Processing cuenta con librerías de gráficos para crear líneas, círculos, cuadrados, curvas, librería para manejo de figuras 3D, protocolos de comunicación como UDP, e incluye la librería para comunicación serial.
[code language=»cpp»]
import processing.serial.*;
Serial myPort; //Crea objeto a partir de la clase Serial
boolean firstSample = true;
float [] RwAcc = new float[3]; // Proyección de vector de fuerza gravitacional en los ejes x/y/z, medidos por el acelerometro
float [] Gyro = new float[3]; //Lectura de Giroscopio
float [] RwGyro = new float[3]; //Rw obtenido del ultimo movimiento y valor estimado del giroscopio
float [] Awz = new float[2]; //angulos entre proyeccion de R en el plano de XZ/YZ y el eje Z (grados)
float [] RwEst = new float[3];
int lastTime = 0;
int interval = 0;
float wGyro = 10.0;
int lf = 10; // el valor decimal 10 es ‘\n’ en ASCII
byte[] inBuffer = new byte[22]; //Numero de caracteres en cada línea que se recibe del arduino (incluye /r/n)
PFont font;
final int VIEW_SIZE_X = 600, VIEW_SIZE_Y = 600;
void setup()
{
size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
myPort = new Serial(this, "COM53", 115200);
font = loadFont("CourierNew36.vlw");
delay(100); // Carga tipo de fuente, debe está localizado en directorio "data" del sketch
myPort.clear();
myPort.write("start");
}
float decodeFloat(String inString) {
byte [] inData = new byte[4];
inString = inString.substring(2, 10); // Descarta "f:" de la cadena inData[0] = (byte) unhex(inString.substring(0, 2));
inData[1] = (byte) unhex(inString.substring(2, 4));
inData[2] = (byte) unhex(inString.substring(4, 6));
inData[3] = (byte) unhex(inString.substring(6, 8));
int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
return Float.intBitsToFloat(intbits);
}
void readSensors() {
if(myPort.available() >= 18) {
String inputString = myPort.readStringUntil((int) ‘\n’);
if (inputString != null && inputString.length() > 0) {
String [] inputStringArr = split(inputString, ",");
RwAcc[0] = decodeFloat(inputStringArr[0]);
RwAcc[1] = decodeFloat(inputStringArr[1]);
RwAcc[2] = decodeFloat(inputStringArr[2]);
Gyro[0] = decodeFloat(inputStringArr[3]);
Gyro[1] = decodeFloat(inputStringArr[4]);
Gyro[2] = decodeFloat(inputStringArr[5]);
RwGyro[0] = decodeFloat(inputStringArr[6]);
RwGyro[1] = decodeFloat(inputStringArr[7]);
RwGyro[2] = decodeFloat(inputStringArr[8]);
Awz[0] = decodeFloat(inputStringArr[9]);
Awz[1] = decodeFloat(inputStringArr[10]);
RwEst[0] = decodeFloat(inputStringArr[11]);
RwEst[1] = decodeFloat(inputStringArr[12]);
RwEst[2] = decodeFloat(inputStringArr[13]);
}
}
}
void buildBoxShape() {
//box(60, 10, 40);
noStroke();
beginShape(QUADS);
//Z+ (to the drawing area)
fill(#00ff00);
vertex(-30, -5, 20);
vertex(30, -5, 20);
vertex(30, 5, 20);
vertex(-30, 5, 20);
//Z-
fill(#0000ff);
vertex(-30, -5, -20);
vertex(30, -5, -20);
vertex(30, 5, -20);
vertex(-30, 5, -20);
//X-
fill(#ff0000);
vertex(-30, -5, -20);
vertex(-30, -5, 20);
vertex(-30, 5, 20);
vertex(-30, 5, -20);
//X+
fill(#ffff00);
vertex(30, -5, -20);
vertex(30, -5, 20);
vertex(30, 5, 20);
vertex(30, 5, -20);
//Y-
fill(#ff00ff);
vertex(-30, -5, -20);
vertex(30, -5, -20);
vertex(30, -5, 20);
vertex(-30, -5, 20);
//Y+
fill(#00ffff);
vertex(-30, 5, -20);
vertex(30, 5, -20);
vertex(30, 5, 20);
vertex(-30, 5, 20);
endShape();
}
void drawCube() {
pushMatrix();
translate(300, 450, 0);
scale(4,4,4);
rotateX(HALF_PI * -RwEst[0]);
rotateZ(HALF_PI * RwEst[1]);
buildBoxShape();
popMatrix();
}
void draw() {
readSensors();
background(#000000);
fill(#ffffff);
textFont(font, 20);
text("RwAcc (G):\n" + RwAcc[0] + "\n" + RwAcc[1] + "\n" + RwAcc[2] + "\ninterval: " + interval, 20, 50);
text("Gyro (°/s):\n" + Gyro[0] + "\n" + Gyro[1] + "\n" + Gyro[2], 220, 50);
text("Awz (°):\n" + Awz[0] + "\n" + Awz[1], 420, 50);
text("RwGyro (°/s):\n" + RwGyro[0] + "\n" + RwGyro[1] + "\n" + RwGyro[2], 20, 180);
text("RwEst :\n" + RwEst[0] + "\n" + RwEst[1] + "\n" + RwEst[2], 220, 180);
// Muestra ejes
pushMatrix();
translate(450, 250, 0);
stroke(#ffffff);
scale(100, 100, 100);
line(0,0,0,1,0,0);
line(0,0,0,0,-1,0);
line(0,0,0,0,0,1);
line(0,0,0, -RwEst[0], RwEst[1], RwEst[2]);
popMatrix();
drawCube();
}
[/code]
float decodeFloat(String inString)
Recibe un string la cual contiene los valores de ejes ya sea del acelerómetro o giroscopio
Esta tutorial es propiedad de HeTPro. Queda expresamente prohibida la reproducción total o parcial por cualquier medio o soporte de los contenidos de esta publicación sin la autorización expresa del editor, cualquier duda, sugerencia o permiso para redistribuir el material favor de mandar un correo a contacto@hetpro.com.mx
Si la redistribución de este material es para fines educativos, difusión tecnológica o cualquier otro fin sin lucro, HeTPro está de acuerdo en que el material sea distribuido sin la necesidad de requerir el permiso del autor.
Bibliografia:
http://www.slideshare.net/berthatonks/acelermetro
http://fuenteabierta.teubi.co/2013/03/inclinometro-digital-con-arduino-uso-de.html
http://blog.make-a-tronik.com/que-es-processing/
Hola, para este tutoriales que codigos usan?
Uso el Ejemplo 29 y el codigo de sketch 131118 para el processing pero me salen los siguientes errores http://prntscr.com/4y5mb2 y parece que no capta valores del integrado
Daniel que tal, mas bien parece que hay un problema con las bibliotecas de dibujo de openGL, puedes probar el puro modulo MPU6050 con la terminal de Arduino y nos dices como te fue.
Disculpa Hector cuales son los nombres del codigo correcto de arduino y processing
El nombre? igual el que tu le des, el nombre del proyecto en este caso no es tan vital.
como se que puerto utilizar?
ASD utiliza el puerto al que corresponde el protocolo de comunicación I2C. Checa tu tarjeta o microcontrolador y busca donde están los pines de I2C (TWI también se le conoce). Saludos
me tira el siguiente error:
OpenGL error 1280 at bot beginDraw(): invalid enumerant
java.lang.NullPointerException
at java.io.DataInputStream.readInt(DataInputStream.java:387)
at processing.core.PFont.(PFont.java:350)
at processing.core.PApplet.loadFont(PApplet.java:6077)
at sketch_150930a.setup(sketch_150930a.java:47)
at processing.core.PApplet.handleDraw(PApplet.java:2373)
at processing.opengl.PSurfaceJOGL$DrawListener.display(PSurfaceJOGL.java:757)
at jogamp.opengl.GLDrawableHelper.displayImpl(GLDrawableHelper.java:691)
at jogamp.opengl.GLDrawableHelper.display(GLDrawableHelper.java:673)
at jogamp.opengl.GLAutoDrawableBase$2.run(GLAutoDrawableBase.java:442)
at jogamp.opengl.GLDrawableHelper.invokeGLImpl(GLDrawableHelper.java:1277)
at jogamp.opengl.GLDrawableHelper.invokeGL(GLDrawableHelper.java:1131)
at com.jogamp.newt.opengl.GLWindow.display(GLWindow.java:680)
at com.jogamp.opengl.util.AWTAnimatorImpl.display(AWTAnimatorImpl.java:77)
at com.jogamp.opengl.util.AnimatorBase.display(AnimatorBase.java:451)
at com.jogamp.opengl.util.FPSAnimator$MainTask.run(FPSAnimator.java:178)
at java.util.TimerThread.mainLoop(Timer.java:555)
at java.util.TimerThread.run(Timer.java:505)
: S
Saludos
No estamos seguros pero me suena a lo siguiente: Corre el processign como administrador y checa que tengas acutalizado el Java. Espero que eso resuelva tus problemas con el programa, igual nos comentas si todo salio bien. Saludos
Hola Hector.
El código de processing produce el siguiente error:
StringIndexOutOfBoundsException: String Index Out of Range:4.
Esto es debido a que la primera posición del array (inputStringArr[0]) no tiene longitud 10 sino 15,luego cuando entra en el procedimiento decodeFloat se produce el error.
Cuando desde arduino se escribe la primera posición le entran 5 bytes de basura.
Se me ocurre testear la longitud antes de hacer la llamada al procedimiento.
Un saludo
Hola Hector!, soy Federico de Argentina, excelente tu explicacion y sketch!!! yo logre que funcione y se estabilicen todos los valores logrando que esté el mpu-6050 este realmente estabilizado. Pero queria consultarte ya que no he encontrado la respuesta ni en el frabricante ni en la web, se puede tomar la Acc Raw de X y determinar si el objeto se mueve hacia adelante o hacia atras ?, hice un codigo para lograrlo pero al parecer tiene mucha oscilacion. Por ejemplo tomo el eje X como mi «camino», si muevo el objeto hacia adelante (SIN ROTARLO) por mi camino, el sensor Raw AccX es positivo y si voy hacia atras es negativo, pero al rotarlo o hacerlo oscilar los valores se disparan solos. Es posible hacer algun filtro para determinar solo el desplazamiento en el eje antes mencionado ? saludos y muchas gracias.
Federico, si, los filtros tienen aplicaciones para este tipo de proyectos. Te recomiendo mucho el filtro Kalman para esto, puedes encontrar mas de el mismo aqui: http://forum.arduino.cc/index.php?topic=58048.0
Hola, necesito el codigo del MPU6050 para arduino DUE.
Solo contamos con este
Buenas amigo, aplico este codigo pero todos los valores en el arduino son ceros. Uso un arduino nano, en sus pines I2C
Angel, si te esta dando ceros no hay comunicación. 1) revisa que el SDA y SCL esten conectados tal cual el mismo nombre en los 2 dispositivos. 2) Revisa las resistencias de Pull up. 3) Revisa la alimentación del dispositivo. 4) Revisa la dirección del mismo, este error es muy comun ya que puede venir configurado para otra dirección.