PRIMER MUESTREO DE LA IMU

Dependencies:   mbed

main.cpp

Committer:
acastiblancoc
Date:
2019-08-24
Revision:
0:0b4ace0e76db

File content as of revision 0:0b4ace0e76db:

#include "mbed.h"

Serial pc(SERIAL_TX, SERIAL_RX);    // (TX, RX)
I2C i2c(PB_9, PB_8 );           // (SDA, SCL)
DigitalIn boton (USER_BUTTON);
const int addr = 0xD0;      // 7 bit I2C address. Accede a la comunicacion I2C
char cmd[2];
char read_buffer[14];
int16_t acc_x = 0, acc_y = 0, acc_z = 0;
int16_t gyr_x = 0, gyr_y = 0, gyr_z = 0;
int16_t temp = 0;
int flag=0;
float ACC_SEN = 16384.0;    //Resolución ACC
float GYR_SEN = 131.0;      //Resolución GYR
float TEM_SEN = 340.0;      //Resolución temp

float facc_x = 0, facc_y = 0, facc_z = 0;
float fgyr_x = 0, fgyr_y = 0, fgyr_z = 0;
float ftemp = 0;

DigitalOut myled(LED1);


void LECTURA_DATOS100 ()
{      
        for(int i=0; i<=100;i++)
        {
        wait_ms(10);
        cmd[0]=0x3B;
        i2c.write(addr, cmd, 1);            //Escritura del registro de inicio, comienza a revisar los diferentes registros segun su orden. 
                                            //Por eso el orden de impresion y el tamaño del buffer (14)
        i2c.read(addr, read_buffer, 14);    //Lectura en rafaga de los valores de la MPU

        //.................Construcción de la medición de los valores .................. 
        acc_x = read_buffer[0]<<8 | read_buffer[1];    
        acc_y = read_buffer[2]<<8 | read_buffer[3];
        acc_z = read_buffer[4]<<8 | read_buffer[5];
        temp  = read_buffer[6]<<8 | read_buffer[7];
        gyr_x = read_buffer[8]<<8 | read_buffer[9];
        gyr_y = read_buffer[10]<<8 | read_buffer[11];
        gyr_z = read_buffer[12]<<8 | read_buffer[13];
        
        facc_x = acc_x/ACC_SEN;
        facc_y = acc_y/ACC_SEN;
        facc_z = acc_z/ACC_SEN;
        fgyr_x = gyr_x/GYR_SEN;
        fgyr_y = gyr_y/GYR_SEN;
        fgyr_z = gyr_z/GYR_SEN;
        ftemp= ((temp)/TEM_SEN)+36.53;               
        pc.printf("ACCx = %.2f ACCy = %.2f ACCz = %.2f GYRx = %.2f GYRy = %.2f GYRz = %.2f Temp = %.2f \r\n  ", facc_x, facc_y, facc_z, fgyr_x, fgyr_y, fgyr_z, ftemp);
        pc.printf("MUESTRA NO. %i\n", i);
        pc.printf("MUESTRA NO. %i\n", i);
    
        }
    }



void LECTURA_DATOS500 ()
{      
        for(int i=0; i<=500;i++)
        {
        wait_ms(10);
        cmd[0]=0x3B;
        i2c.write(addr, cmd, 1);            //Escritura del registro de inicio, comienza a revisar los diferentes registros segun su orden. 
                                            //Por eso el orden de impresion y el tamaño del buffer (14)
        i2c.read(addr, read_buffer, 14);    //Lectura en rafaga de los valores de la MPU

        //.................Construcción de la medición de los valores .................. 
        acc_x = read_buffer[0]<<8 | read_buffer[1];    
        acc_y = read_buffer[2]<<8 | read_buffer[3];
        acc_z = read_buffer[4]<<8 | read_buffer[5];
        temp  = read_buffer[6]<<8 | read_buffer[7];
        gyr_x = read_buffer[8]<<8 | read_buffer[9];
        gyr_y = read_buffer[10]<<8 | read_buffer[11];
        gyr_z = read_buffer[12]<<8 | read_buffer[13];
        
        facc_x = acc_x/ACC_SEN;
        facc_y = acc_y/ACC_SEN;
        facc_z = acc_z/ACC_SEN;
        fgyr_x = gyr_x/GYR_SEN;
        fgyr_y = gyr_y/GYR_SEN;
        fgyr_z = gyr_z/GYR_SEN;
        ftemp= ((temp)/TEM_SEN)+36.53;               
        pc.printf("ACCx = %.2f ACCy = %.2f ACCz = %.2f GYRx = %.2f GYRy = %.2f GYRz = %.2f Temp = %.2f \r\n  ", facc_x, facc_y, facc_z, fgyr_x, fgyr_y, fgyr_z, ftemp);
        pc.printf("MUESTRA NO. %i\n", i);
    
        }
    }

int main()
{
    pc.printf("Prueba MPU6050 \n\r");

    cmd[0] = 0x6B;
    cmd[1] = 0x00;
    i2c.write(addr, cmd, 2);                //Desactivar modo hibernación
    
    cmd[0] = 0x1B;
    cmd[1] = 0x00;
    i2c.write(addr, cmd, 2);                //gyro full scale 250 DPS

    cmd[0] = 0x1C;
    cmd[1] = 0x00;
    i2c.write(addr, cmd, 2);                //ACC fullsclae 2G
    
    while(1) {
        
        if(boton==0)
        {
        if (flag==0)
        {LECTURA_DATOS100();
        flag++;}
        else if (flag==1)
        {LECTURA_DATOS500();
        flag=0;
        }
        }
    }
}