main.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 #include "TextLCD.h"
00004 
00005 MPU6050 mpu(A4,A5);
00006 Serial pc(USBTX, USBRX);
00007 TextLCD lcd(PTB18,PTB19,PTC0,PTC4,PTC6,PTC7, TextLCD::LCD16x2 );
00008 Timer timer;
00009 int gx,gy,gz,ax,ay,az;
00010 long tiempo_prev;
00011 float dt,giro1x,giro1z,giro2x,giro2z,giro1y,giro2y;
00012 
00013 int main()
00014 {
00015     timer.start();
00016     if(mpu.getID()==0x68) {
00017         lcd.cls();
00018         lcd.printf("MPU6050 OK");
00019         wait(1);
00020     } else {
00021         lcd.cls();
00022         lcd.printf("MPU6050 error ID=0x%x\r\n",mpu.getID());
00023         while(1) {
00024         }
00025     }
00026     mpu.start();
00027     tiempo_prev=timer.read_ms();
00028     while(1) {
00029 
00030         mpu.readraw(&gx,&gy,&gz,&ax,&ay,&az);
00031         dt =(timer.read_ms() - tiempo_prev);
00032         tiempo_prev=timer.read_ms();
00033        
00034        
00035         giro1x=(gx/131)*dt/1000.0+giro2x;
00036         giro1z=(gz/131)*dt/1000.0+giro2z;
00037         giro1y=(gy/131)*dt/1000.0+giro2y;
00038         giro2x=giro1x;
00039         giro2z=giro1z;
00040         giro2y=giro1y;
00041        lcd.cls();
00042         lcd.printf("z: %f",giro1z);
00043        wait(1);
00044        lcd.cls();
00045         lcd.printf("x: %f",giro1x);
00046        wait(1);
00047         lcd.cls();
00048         lcd.printf("y: %f ",giro1y);
00049        wait(1);
00050         //pc.printf("Der: %5.1f ,Izq: %5.1f ",tDer,tIzq);
00051         
00052     }
00053 }
00054 
00055 //////////////////////////////////////////////////////////////////////////////////////////////////////
00056 
00057