Diff: main.cpp

Revision:

2:01ca44dd3908

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Dec 04 20:24:04 2018 +0000
@@ -0,0 +1,150 @@
+
+
+#include "mbed.h"
+#include "MPU6050.h"
+#include "math.h"
+MPU6050 mpu(D4,D5);
+Serial pc(USBTX, USBRX); // tx, rx
+Timer t;
+Serial  serial(PA_9,PA_10);
+char    data[30];
+
+//DigitalIn res(USER_BUTTON);
+
+float ark[3];
+    float gy[3];
+    
+    int timestamp=0;
+    float Now,lastupdate;
+float cali_ax,cali_ay,cali_az,cali_gx,cali_gy,cali_gz;
+    static float v[3]={0,0,0};
+    
+float PI = 3.14159265358979323846f;
+void MPUcalibate(){
+    static float axx=0,ayy=0,azz=0,gxx=0,gyy=0,gzz=0;
+    int x=0;
+    while(x<=9999) {
+        mpu.getAccelero(ark);
+        mpu.getGyro(gy);
+        axx+=ark[0];
+        ayy+=ark[1];
+        azz+=ark[2];
+        gxx+=gy[0];
+        gyy+=gy[1];
+        gzz+=gy[2];
+        x+=1;
+        wait(0.0001);
+    }
+    cali_ax=axx/10000.0f;
+    cali_ay=ayy/10000.0f;
+    cali_az=azz/10000.0f;
+    cali_gx=gxx/10000.0f;
+    cali_gy=gyy/10000.0f;
+    cali_gz=gzz/10000.0f;
+    //pc.printf("%.10f %.10f %.10f\n",cali_ax,cali_ay,cali_az-9.80665);
+    //pc.printf("%.10f %.10f %.10f\n",cali_ax-9.80665,cali_ay,cali_az);
+    //pc.printf("%.10f %.10f %.10f\n",cali_ax,cali_ay-9.80665,cali_az);
+    //pc.printf("%f %f %f\n",cali_gx,cali_gy,cali_gz);
+    }
+void AccelToVelo(float ax,float ay,float az,float dt){
+    v[0]+=(ax*dt/129); //(-156.10655212402344+150.74868774414062);
+    v[1]+=(ay*dt/180); //(-208.04095458984375+194.97883605957031);
+    v[2]+=(az*dt/6); //(151.52879333496094-144.20004272460938);
+    }
+/*float CalculateAngle(float ax,float ay,float az,float gx,float gy,float gz){
+    float pitch;
+    gx=0;
+    gy=0;
+    gz=0;
+    static float fillter_pitch=0;
+    fillter_pitch=(fillter_pitch*0.8)+(pitch*0.2);
+    pitch=atan((ax)/sqrt((ay)*(ay)+(az)*(az))*(180/PI));
+    //pc.printf("%f\n",pitch);
+    return fillter_pitch;
+    }*/
+int main()
+{
+    pc.baud(115200);
+    static float calibate_acc[3]={0.6465099045, 0.14739338841, -0.2634094528};
+    static float calibate_gy[3]={-0.022125, 0.005254 ,-0.009761};
+    mpu.getAccelero(ark);
+        mpu.getGyro(gy);
+    //CalculateAngle(ark[0],ark[1],ark[2],0,0,0);
+    static float fillter_pitch=0;
+    int co=0;
+    float pitch;
+    float delay=0.0001,current=0.0,deltat;
+    t.start();
+    
+    wait(0.5);
+    
+    //MPUcalibate();
+    //cali_err=atan((-(ark[1]-cali_ay))/(ark[2]-cali_az));
+    //cali_p=atan2(((ark[2]-cali_az)),(ark[0]-cali_ax));
+    //cali_p=atan((ark[0]-cali_ax)/sqrt((ark[1]-cali_ay)*(ark[1]-cali_ay)+(ark[2]-cali_az)*(ark[2]-cali_az)));
+    //pc.printf("cali P %f \n",cali_err);
+    //wait(1);
+      /*  fillter_pitch2=(fillter_pitch2*0.75)+(fillter_pitch*0.25);
+    fillter_pitch3=(fillter_pitch3*0.75)+(fillter_pitch2*0.25);
+    fillter_pitch4=(fillter_pitch4*0.75)+(fillter_pitch3*0.25);
+    fillter_pitch5=(fillter_pitch5*0.75)+(fillter_pitch4*0.25);
+    fillter_pitch6=(fillter_pitch6*0.75)+(fillter_pitch5*0.25);
+    fillter_pitch7=(fillter_pitch7*0.75)+(fillter_pitch6*0.25);
+    fillter_pitch8=(fillter_pitch8*0.75)+(fillter_pitch7*0.25);
+    fillter_pitch9=(fillter_pitch9*0.75)+(fillter_pitch8*0.25);
+    fillter_pitch10=(fillter_pitch10*0.75)+(fillter_pitch9*0.25);*/
+    while(1) {
+        Now = t.read();
+        deltat=Now-current;
+        if (deltat>0.1){
+        mpu.getAccelero(ark);
+        mpu.getGyro(gy);
+        ark[0] -= calibate_acc[0];
+        ark[1] -= calibate_acc[1];
+        ark[2] -= calibate_acc[2];
+        gy[0] -= calibate_gy[0];
+        gy[1] -= calibate_gy[1];
+        gy[2] -= calibate_gy[2];
+        float g=sqrt((ark[0]*ark[0])+(ark[1]*ark[1])+(ark[2]*ark[2]));
+        while(co<1000){
+        AccelToVelo((ark[0]),ark[1],ark[2]-9.8065,deltat);
+        co+=1;
+        }
+        co=0;
+        ark[0]+=0.5;
+        float gra=sqrt((ark[0]*ark[0])+(ark[1]*ark[1])+(ark[2]*ark[2]));
+        pc.printf("Q AX %f AY %f AZ %f G %f \n",ark[0],ark[1],ark[2],gra);
+        if(serial.writeable()) {
+            serial.printf("%f %f %f %f \n",ark[0],ark[1],ark[2],gra);
+            //serial.printf("------------------------ \n");
+            //serial.printf("   Gyx:%f_Gyy:%f_Gyz:%f \n",gy[0],gy[1],gy[2]);
+            wait_ms(500);   // lag to debounce the button
+    }
+        //pc.printf("Q GyX %f GyY %f GyZ %f \n",gy[0],gy[1],gy[2]);
+        //pc.printf("Q Vx %f Vy %f Vz %f \n",v[0],v[1],v[2]);
+        //pc.printf("%.14f\n",v[1]/100);
+        pc.printf("Q AX %f AY %f AZ %f GyX %f GyY %f GyZ %f \n",ark[0]+0.5,ark[1],ark[2],gy[0],gy[1],gy[2]);
+        //timestamp+=1;
+        //pc.printf("%f\n",atan((-(ark[1]-cali_ay))/(ark[2]-cali_az)));
+        //pc.printf("Ax %f\n",(atan2((ark[2]-cali_az),(ark[0]-cali_ax))-cali_p)*(180/PI));
+        //pitch=atan((ark[0])/sqrt((ark[1])*(ark[1])+(ark[2])*ark[2]))*(180/PI);
+        //CalculateAngle(ark[0],ark[1],ark[2],0,0,0);
+        //pc.printf("%lf\n",CalculateAngle(ark[0],ark[1],ark[2],0,0,0));
+        pitch=atan(ark[0]/sqrt(pow(ark[1],2)+pow(ark[2],2)))*(180/PI);
+    //fillter_pitch=(fillter_pitch*0.75)+(pitch*0.25);
+   // fillter_pitch2=(fillter_pitch2*0.75)+(fillter_pitch*0.25);
+   // pc.printf("%f\n",fillter_pitch);
+        //pc.printf("Ax %f\n",(atan((ark[0])/sqrt((ark[1]-cali_ay)*(ark[1]-cali_ay)+(ark[2]-cali_az)*(ark[2]-cali_az)))-cali_p)*(180/PI));
+       // pc.printf("%.10f %.10f %.10f\n",ark[0],ark[1],ark[2]);
+        //pc.printf("%f %f %f\n",gy[0],gy[1],gy[2]);
+        //pc.printf("%.2f\n",t.read());
+        wait(0.02);
+        /*if (res==0) {
+            //lastupdate=t.read()-Now;
+            //Now=lastupdate;
+            t.reset();
+        }*/
+current=Now;
+}
+    }
+}