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.


SENSOR IMU 9GDL


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

Entradas más populares de este blog

5. CAPITULO 4. MOTOR PASO A PASO

CAPITULO 7. SENSOR DE HUELLA DACTILAR R307

2. CAPITULO 1 SENSORES (Ultrasonido SR04)