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/