der

Dependencies:   mbed MPU6050 TextLCD

main.cpp

Committer:
20172573073
Date:
2020-05-06
Revision:
0:304aaa70b2e8

File content as of revision 0:304aaa70b2e8:

#include "mbed.h"
#include "MPU6050.h"
#include "TextLCD.h"

MPU6050 mpu(A4,A5);
Serial pc(USBTX, USBRX);
TextLCD lcd(PTB18,PTB19,PTC0,PTC4,PTC6,PTC7, TextLCD::LCD16x2 );
Timer timer;
int gx,gy,gz,ax,ay,az;
long tiempo_prev;
float dt,giro1x,giro1z,giro2x,giro2z,giro1y,giro2y;

int main()
{
    timer.start();
    if(mpu.getID()==0x68) {
        lcd.cls();
        lcd.printf("MPU6050 OK");
        wait(1);
    } else {
        lcd.cls();
        lcd.printf("MPU6050 error ID=0x%x\r\n",mpu.getID());
        while(1) {
        }
    }
    mpu.start();
    tiempo_prev=timer.read_ms();
    while(1) {

        mpu.readraw(&gx,&gy,&gz,&ax,&ay,&az);
        dt =(timer.read_ms() - tiempo_prev);
        tiempo_prev=timer.read_ms();
       
       
        giro1x=(gx/131)*dt/1000.0+giro2x;
        giro1z=(gz/131)*dt/1000.0+giro2z;
        giro1y=(gy/131)*dt/1000.0+giro2y;
        giro2x=giro1x;
        giro2z=giro1z;
        giro2y=giro1y;
       lcd.cls();
        lcd.printf("z: %f",giro1z);
       wait(1);
       lcd.cls();
        lcd.printf("x: %f",giro1x);
       wait(1);
        lcd.cls();
        lcd.printf("y: %f ",giro1y);
       wait(1);
        //pc.printf("Der: %5.1f ,Izq: %5.1f ",tDer,tIzq);
        
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////