4. CAPITULO 3 SENSORES (IMU 9GDL)
Un sensor de Unidad de Mediad Inercial con 9 Grados De Libertad (conocido como IMU 9GDL) es un sensor embebido que integra un acelerómetro, un magnetómetro y un giroscopio cada uno de tres ejes dentro de un mismo módulo electrónico.
Este sensor es muy usado en el mundo de la robótica y cibertrónica tales como los vehículos operados remotamente en aviones, drones, barcos o en carros que requieran de posición en el espacio a controlar.
La IMU de 9 GDL viene de diversas presentaciones tales como el GY-85 y un nuevo modeulo IMU 9GDL más económico que integran los chips LSM303DLHC para el magnetómetro y acelerómetro y el L3GD20 para el giroscopio.
Este sensor se alimenta entre 3 a 5 voltios DC, la comunicación que ejerce con el microcontrolador (por ejemplo el Atmega 328 que se encuentra dentro de la tarjeta Arduino Uno) es mediante el protocolo I2C (en otra sección explicaremos con mas detalles). En caso de tener una Arduino Uno R3 CH340, se debe programar cada uno por separado teniendo en cuenta varias librerías:
a) Wire.h --> libreria usada para activar el I2C
b) ADLX345.h --> librería para el acelerómetro
c) HMC5883L.h --> libreria para el magnetómetro
d) ITG3200.h --> libreria para el giroscopio
Antes de copiar y pegar el codigo facilitado, es necesario que sepa que se requiere que averigue la declinación magnetica para el magnetómetro, este puede encontrarlo en la URL (http://magnetic-declination.com/) para mayor exactitud.
CÓDIGO ARDUINO
//**********Comunicación I2C*************
#include <Wire.h>
//**********IMU 9GDL*************
#include <ADXL345.h> // ADXL345 Accelerometer Library
#include <HMC5883L.h> // HMC5883L Magnetometer Library
#include <ITG3200.h> // ITG3200 Giroscope Library
ADXL345 acc; //variable adxl is an instance of the ADXL345 library
HMC5883L compass;
int16_t mx, my, mz;
//float declinacion=-0.1221; //declinación de -1°16'(Trujillo-Perú)
ITG3200 gyro = ITG3200();
float gx,gy,gz;
float gx_rate, gy_rate, gz_rate;
int ix, iy, iz;
float anglegx=0.0, anglegy=0.0, anglegz=0.0;
int ax,ay,az;
int rawX, rawY, rawZ;
float X, Y, Z;
float rollrad, pitchrad;
float rolldeg, pitchdeg;
int error = 0;
float aoffsetX, aoffsetY, aoffsetZ;
float goffsetX, goffsetY, goffsetZ;
unsigned long time, looptime;
void setup() {
// put your setup code here, to run once:
//*********PUERTO SERIE************
Serial.begin(9600);
//**********PUERTO I2C************
Wire.begin(); // Inicia el puerto I2C
delay(1000);
//***************************************
//*********IMU 9GDL**********************
//***************************************
//*******CONFIGURACIÓN MAGNETÓMETRO********
// Initialize Initialize HMC5883L
Serial.println("Initialize HMC5883L");
while (!compass.begin())
{
Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
delay(500);
}
// Set measurement range
compass.setRange(HMC5883L_RANGE_1_3GA);
// Set measurement mode
compass.setMeasurementMode(HMC5883L_CONTINOUS);
// Set data rate
compass.setDataRate(HMC5883L_DATARATE_30HZ);
// Set number of samples averaged
compass.setSamples(HMC5883L_SAMPLES_8);
// Set calibration offset. See HMC5883L_calibration.ino
compass.setOffset(0, 0);
//************CONFIGURACIÓN DEL ACELERÓMETRO***********
acc.powerOn();
for (int i = 0; i <= 200; i++) {
acc.readAccel(&ax, &ay, &az);
if (i == 0) {
aoffsetX = ax;
aoffsetY = ay;
aoffsetZ = az;
}
if (i > 1) {
aoffsetX = (ax + aoffsetX) / 2;
aoffsetY = (ay + aoffsetY) / 2;
aoffsetZ = (az + aoffsetZ) / 2;
}
}
for (int i = 0; i <= 200; i++) {
gyro.readGyro(&gx,&gy,&gz);
if (i == 0) {
goffsetX = gx;
goffsetY = gy;
goffsetZ = gz;
}
if (i > 1) {
goffsetX = (gx + goffsetX) / 2;
goffsetY = (gy + goffsetY) / 2;
goffsetZ = (gz + goffsetZ) / 2;
}
}
Serial.println("Done HMC5883L");
delay(1000); //Estaba en delay(1000);
Serial.println("Start with Giroscope");
gyro.init(ITG3200_ADDR_AD0_LOW);
Serial.println("zero Calibrating.");
delay(1000); //Estaba en delay(1000);
Serial.println("zero Calibrating..");
delay(1000); //Estaba en delay(1000);
Serial.println("zero Calibrating...");
gyro.zeroCalibrate(2500, 2); //(number of measurements, milliseconds between them)
Serial.println("done.");
delay(1000);
}
void loop() {
// put your main code here, to run repeatedly:
//************IMU 9GDL*********************
updateData();
// Accelerometer, Magnetometer and Gyroscope Output
Serial.print("Roll:" );
Serial.print(rolldeg);
Serial.print("\t");
Serial.print("Pitch:");
Serial.print(pitchdeg); // calculated angle in degrees
Vector norm = compass.readNormalize();
// Calculate heading
float heading = atan2(norm.YAxis, norm.XAxis);
// Calculate yaw
float yawRad=atan((norm.XAxis*sin(rollrad)*sin(pitchrad)+norm.YAxis*cos(rollrad)-norm.ZAxis*sin(rollrad)*cos(pitchrad))/(norm.XAxis*cos(pitchrad)+norm.ZAxis*sin(pitchrad)));
// Convert to degrees
float yawDeg=(180*yawRad)/PI;
// Set declination angle on your location and fix heading
// You can find your declination on: http://magnetic-declination.com/
// (+) Positive or (-) for negative
// For Bytom / Poland declination angle is 4'26E (positive)
// For Cartagena / Turbaco declination angle is -7'6W (negative)
// Formula: (deg + (min / 60.0)) / (180 / M_PI);
//float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / M_PI);
float declinationAngle = (-7.0 + (6.0 / 60.0)) / (180 / M_PI);
heading += declinationAngle;
// Correct for heading < 0deg and heading > 360deg
if (heading < 0)
{
heading += 2 * PI;
}
if (heading > 2 * PI)
{
heading -= 2 * PI;
}
// Convert to degrees
float headingDegrees = heading * 180/M_PI;
// Output compass
Serial.print("\t");
Serial.print("Brujula[Rad] = ");
Serial.print(heading);
Serial.print("\t");
Serial.print("Brujula[Grados]= ");
Serial.print(headingDegrees);
Serial.print("\t");
Serial.print("Yaw[Rad]= ");
Serial.print(yawRad);
Serial.print("\t");
Serial.print("Yaw[Deg]= ");
Serial.print(yawDeg);
Serial.print("\t");
// Normally we would delay the application by 66ms to allow the loop
// to run at 15Hz (default bandwidth for the HMC5883L).
// However since we have a long serial out (104ms at 9600) we will let
// it run at its natural speed.
// delay(66);
Serial.print("GiroX[Deg]: ");
Serial.print(gx_rate); // calculated angle in degrees
Serial.print("\t");
Serial.print("GiroY[Deg]: ");
Serial.print(gy_rate);
Serial.print("\t");
Serial.print("GiroZ[Deg]: ");
Serial.print(gz_rate);
Serial.print("\t");
Serial.print("GiroX[Norm]: ");
Serial.print(anglegx);
Serial.print("\t");
Serial.print("AnguloY[Norm]: ");
Serial.print(anglegy);
Serial.print("\t");
Serial.print("AnguloZ[Norm]: ");
Serial.print(anglegz);
Serial.print("\t");
Serial.print("Looptime: ");
Serial.print(looptime);
Serial.println("\t");
}
void updateData(){
// code fragment for Accelerometer angles (roll and pitch)
time = millis();
acc.readAccel(&ax, &ay, &az); //read the accelerometer values and store them in variables x,y,z
rawX = ax - aoffsetX;
rawY = ay - aoffsetY;
rawZ = az - (255 - aoffsetZ);
X = rawX / 256.00; // used for angle calculations
Y = rawY / 256.00; // used for angle calculations
Z = rawZ / 256.00; // used for angle calculations
rollrad = atan(Y / sqrt(X * X + Z * Z)); // calculated angle in radians
pitchrad = atan(X / sqrt(Y * Y + Z * Z)); // calculated angle in radians
rolldeg = 180 * (atan(Y / sqrt(X * X + Z * Z))) / PI; // calculated angle in degrees
pitchdeg = 180 * (atan(X / sqrt(Y * Y + Z * Z))) / PI; // calculated angle in degrees
// Code fragment for Gyroscope (roll, pitch, yaw)
gyro.readGyro(&gx, &gy, &gz);
looptime = millis() - time;
gx_rate = (gx - goffsetX) / 14.375;
gy_rate = (gy - goffsetY) / 14.375;
gz_rate = (gz - goffsetZ) / 14.375;
delay(100);
anglegx = anglegx + (gx_rate * looptime);
anglegy = anglegy + (gx_rate * looptime);
anglegz = anglegz + (gz_rate * looptime);
}
CIRCUITO DE CONEXIÓN EN PROTEUS
Puedes contactarme: Ing. David Muñoz Aldana
<- ->
Comentarios
Publicar un comentario