Diff: main.cpp

Revision:

0:9dc7ab9daa11

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Aug 22 11:34:26 2018 +0000
@@ -0,0 +1,69 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "C12832.h"
+
+AnalogOut Aout(p18);
+DigitalOut myled(LED1);
+Serial pc(USBTX, USBRX);
+MPU6050 mpu;
+C12832 lcd(p5, p7, p6, p8, p11);
+ 
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+
+float k=0.5;
+float x=0;
+float xout=0,xold=0;
+float x_prev=0;
+float sum =0;
+float diff =0;
+float gain_prop = 1;
+float gain_int = 0.0066;
+float gain_diff = 0.000028;
+
+ 
+int main()
+{
+    lcd.printf("MPU6050 test\n\n");
+    lcd.printf("MPU6050 initialize \n");
+    
+    mpu.initialize();
+    
+    pc.printf("MPU6050 testConnection \n");
+ 
+    bool mpu6050TestResult = mpu.testConnection();
+    if(mpu6050TestResult) {pc.printf("MPU6050 test passed \n");}
+    else {pc.printf("MPU6050 test failed \n");}
+    float proportional,integral=0;
+    float prop=0;
+    float xAcc = 0;
+    unsigned char i;
+    signed int Reading, xsum=0;
+    lcd.cls();
+    while(1)
+    {
+        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+        //lcd.locate(0,15);
+        //writing current accelerometer and gyro position
+        for(i=1; i<5; i++)
+        {
+                Reading = ay;
+                xsum += Reading;
+        }
+        xAcc=Reading*0.0001/4;
+        //x=ay*0.0001;
+        diff=gx*0.001;
+        proportional = 1.0 + xAcc * 1.31;/*x*/
+        prop = proportional/3.3;
+        sum = sum +  xAcc ;/*x*/
+        integral = sum;
+        xout = prop*gain_prop + integral*gain_int + diff*gain_diff;
+        xout=0.5*(xold) +(1-0.5)*xout;
+        Aout=xout;
+        xold=xout;
+        
+        
+        //lcd.printf(" Xaxis :%.5f",proportional);
+
+    }
+}