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.