Diff: main.cpp

Revision:

0:304aaa70b2e8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 06 03:15:37 2020 +0000
@@ -0,0 +1,57 @@
+#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);
+        
+    }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+
+