Pages Menu
TwitterFacebook



BANNER1_ALT

Posted by on Abr 25, 2014 in Sensores digitales | 12 comments

Acelerometro MPU-6050

Acelerometro MPU-6050

 

 

 

  MPU-6050 

SMPU6050

 

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

 MPU_6050_4

PINES UTILIZADOS

 

 tabla

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:

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;
}

 

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).

 

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;
}

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)

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;
}

 

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.

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;
}

 

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 )

 

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;
}

 

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.

 

 


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();
}

 

 

float decodeFloat(String inString)

Recibe un string la cual contiene los valores de ejes ya sea del acelerómetro o giroscopio

imu_4

imu_3

imu_2 imu

 

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/

12 Comments

  1. 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.

  2. 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.

  3. 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

  4. 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

  5. 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

  6. 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.

  7. Hola, necesito el codigo del MPU6050 para arduino DUE.

Post a Reply

Tu dirección de correo electrónico no será publicada. Los campos necesarios están marcados *