IMU Visualization

Una vez que hemos aprendido a sacar datos de nuestros sensores MPU, Vamos a proceder con la visualización en 3 dimensiones del comportamiento de este conjunto de números a la hora de ponerlos en práctica.

Como habíamos dicho en un post anterior, debemos de conectar nuestra placa tal y como se indica en la figura y descargar las librerías pertinentes para ponerlas en funcionamiento en la programación de nuestra placa.

Las librerías que requerimos son las siguientes.

Una vez que tengamos conectados los cables e instaladas las librerías podemos proceder a la programación a través de la página de Ardublockly o desde el propio IDE de Arduino.

Processing Code

Para visualizar la respuesta en un entorno 3D, deberemos utilizar Processing, que es un programa similar a Arduino y con el que también podemos programar muchísimas utilidades de nuestro ordenador. En este caso, comunicaremos por el puerto USB la información obtenido de nuestro sensor para ver cómo se comporta en el espacio tridimensional y verificar que todo funciona correctamente.

 

import processing.serial.*;
Serial myPort;

float yaw = 0.0;
float pitch = 0.0;
float roll = 0.0;

void setup()
{
  size(600, 500, P3D);

  // if you have only ONE serial port active
  myPort = new Serial(this, Serial.list()[0], 9600); // if you have only ONE serial port active

  textSize(16); // set text size
  textMode(SHAPE); // set text mode to shape
}

void draw()
{
  serialEvent();  // read and parse incoming serial message
  background(255); // set background to white
  lights();

  translate(width/2, height/2); // set position to centre

  pushMatrix(); // begin object

  float c1 = cos(radians(roll));
  float s1 = sin(radians(roll));
  float c2 = cos(radians(pitch));
  float s2 = sin(radians(pitch));
  float c3 = cos(radians(yaw));
  float s3 = sin(radians(yaw));
  applyMatrix( c2*c3, s1*s3+c1*c3*s2, c3*s1*s2-c1*s3, 0,
               -s2, c1*c2, c2*s1, 0,
               c2*s3, c1*s2*s3-c3*s1, c1*c3+s1*s2*s3, 0,
               0, 0, 0, 1);

  drawArduino();

  popMatrix(); // end of object

  // Print values to console
  print(roll);
  print("\t");
  print(pitch);
  print("\t");
  print(yaw);
  println();
}

void serialEvent()
{
  int newLine = 13; // new line character in ASCII
  String message;
  do {
    message = myPort.readStringUntil(newLine); // read from port until new line
    if (message != null) {
      String[] list = split(trim(message), " ");
      if (list.length >= 4 && list[0].equals("Orientation")) {
        yaw = float(list[1]); // convert to float yaw
        pitch = float(list[2]); // convert to float pitch
        roll = float(list[3]); // convert to float roll
      }
    }
  } while (message != null);
}

void drawArduino()
{
  /* function contains shape(s) that are rotated with the IMU */
  stroke(0, 90, 90); // set outline colour to darker teal
  fill(0, 130, 130); // set fill colour to lighter teal
  box(300, 10, 200); // draw Arduino board base shape

  stroke(0); // set outline colour to black
  fill(80); // set fill colour to dark grey

  translate(60, -10, 90); // set position to edge of Arduino box
  box(170, 20, 10); // draw pin header as box

  translate(-20, 0, -180); // set position to other edge of Arduino box
  box(210, 20, 10); // draw other pin header as box
}

En el momento que tengamos este programa en Processing, solo nos falta programar nuestra placa para comenzar a enviar datos y darle al play para ver cómo se comporta nuestro objeto en 3D.

IMU Visualization

Como se puede apreciar, ahora no utilizamos los datos directamente del IMU, sino que los hacemos pasar a través de un filtro madgwick con frecuencia de 25 microsegundos. Esto es importante para no bloquear el cómputo en el bucle y que lo valores se van actualizando y corrigiendo con suficiente velocidad  para dar precisión en los valores de los ángulos de Roll, Pitch y Yaw.

Se puede acceder al mismo jemplo que se dispone en el menú de la esquina superior izquierda.

 

#include <MadgwickAHRS.h>
#include <IMU_MPU6050.h>

float roll;
float pitch;
float yaw;
long microsPerReading;

Madgwick filter;
MPU6050 imu;


void updateIMU(float *roll, float *pitch, float *yaw){

  if (imu.readByte(MPU6050_ADDRESS, INT_STATUS) && 0x01){ 
    imu.readAccelData(imu.accelCount);
    imu.getAres();

    imu.ax = (float)imu.accelCount[0]*imu.aRes;
    imu.ay = (float)imu.accelCount[1]*imu.aRes;
    imu.az = (float)imu.accelCount[2]*imu.aRes;
    imu.readGyroData(imu.gyroCount);
    imu.getGres();

    imu.gx = (float)imu.gyroCount[0]*imu.gRes;
    imu.gy = (float)imu.gyroCount[1]*imu.gRes;
    imu.gz = (float)imu.gyroCount[2]*imu.gRes;
  }

  imu.updateTime();
  imu.delt_t = millis() - imu.count;
  if (imu.delt_t > microsPerReading){
    filter.updateIMU(imu.gx, imu.gy, imu.gz, imu.ax, imu.ay, imu.az);
    *roll = filter.getRoll();
    *pitch = filter.getPitch();
    *yaw = filter.getYaw();
    imu.count = millis();
  }
}
void setup() {
  Wire.begin();
  Serial.begin(9600);
  filter.begin(25);
  
  microsPerReading = 1000 / 25;
  
  imu.calibrateMPU6050(imu.gyroBias, imu.accelBias);
  imu.initMPU6050();
}

void loop() {
  updateIMU(&roll, &pitch, &yaw);

    Serial.print("Orientation ");
    Serial.print(yaw);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);
  
}

 


 

IMU Visualization Serial non-blocking

Poniendo a prueba el anterior código se podrá observar que el objeto 3D se mueve a cachos y que su respuesta no es fluida. Esto se debe a la acción bloqueante de la impresión por el puerto serie de los datos obtenidos. Esto es un problema asociado a los sistemas de tiempo real y es que a la hora de usar sensores tan críticos como los que calculan la orientación, no podemos permitir que otras funciones se ejecuten de forma recursiva creando retrasos en cada ejecución del bucle.

Y por esta razón, se deduce que los delays son malos.

Para mejorar este código, hay que realizar la siguiente acción, para hacer que esta impresión por el puerto serie no bloquee nuestro robot.

Arduino Code

#include <MadgwickAHRS.h>
#include <IMU_MPU6050.h>

float roll;
float pitch;
float yaw;
long Serialtime;
long last_time;
long microsPerReading;

Madgwick filter;
MPU6050 imu;


void updateIMU(float *roll, float *pitch, float *yaw){

  if (imu.readByte(MPU6050_ADDRESS, INT_STATUS) && 0x01){    imu.readAccelData(imu.accelCount);
    imu.getAres();

    imu.ax = (float)imu.accelCount[0]*imu.aRes;
    imu.ay = (float)imu.accelCount[1]*imu.aRes;
    imu.az = (float)imu.accelCount[2]*imu.aRes;
    imu.readGyroData(imu.gyroCount);
    imu.getGres();

    imu.gx = (float)imu.gyroCount[0]*imu.gRes;
    imu.gy = (float)imu.gyroCount[1]*imu.gRes;
    imu.gz = (float)imu.gyroCount[2]*imu.gRes;
  }

  imu.updateTime();
  imu.delt_t = millis() - imu.count;
  if (imu.delt_t > microsPerReading){
    filter.updateIMU(imu.gx, imu.gy, imu.gz, imu.ax, imu.ay, imu.az);
    *roll = filter.getRoll();
    *pitch = filter.getPitch();
    *yaw = filter.getYaw();
    imu.count = millis();
  }
}
void setup() {
  Wire.begin();
  Serial.begin(9600);
  filter.begin(25);
  
  microsPerReading = 1000 / 25;
  Serialtime = 50;
  last_time = millis();
  
  imu.calibrateMPU6050(imu.gyroBias, imu.accelBias);
  imu.initMPU6050();
}

void loop() {
  updateIMU(&roll, &pitch, &yaw);
  if (millis() - last_time > Serialtime) {
    Serial.print("Orientation ");
    Serial.print(yaw);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);
    last_time = millis();
  }
}

Una vez que comprobemos que los datos de nuestro sensor se comportan adecuadamente para la obtención de la orientación de nuestro robot; es hora de hacer que nuestro robot se mueva en función de estos valores.