Panusorn Chinsakuljaroen / Mbed 2 deprecated BTL432kc

imu for l432kc

Dependencies:   mbed

main.cpp@2:01ca44dd3908, 2018-12-04 (annotated)

Committer:
sunninety1
Date:
Tue Dec 04 20:24:04 2018 +0000
Revision:
2:01ca44dd3908
kk

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sunninety1 2:01ca44dd3908 1
sunninety1 2:01ca44dd3908 2
sunninety1 2:01ca44dd3908 3 #include "mbed.h"
sunninety1 2:01ca44dd3908 4 #include "MPU6050.h"
sunninety1 2:01ca44dd3908 5 #include "math.h"
sunninety1 2:01ca44dd3908 6 MPU6050 mpu(D4,D5);
sunninety1 2:01ca44dd3908 7 Serial pc(USBTX, USBRX); // tx, rx
sunninety1 2:01ca44dd3908 8 Timer t;
sunninety1 2:01ca44dd3908 9 Serial serial(PA_9,PA_10);
sunninety1 2:01ca44dd3908 10 char data[30];
sunninety1 2:01ca44dd3908 11
sunninety1 2:01ca44dd3908 12 //DigitalIn res(USER_BUTTON);
sunninety1 2:01ca44dd3908 13
sunninety1 2:01ca44dd3908 14 float ark[3];
sunninety1 2:01ca44dd3908 15 float gy[3];
sunninety1 2:01ca44dd3908 16
sunninety1 2:01ca44dd3908 17 int timestamp=0;
sunninety1 2:01ca44dd3908 18 float Now,lastupdate;
sunninety1 2:01ca44dd3908 19 float cali_ax,cali_ay,cali_az,cali_gx,cali_gy,cali_gz;
sunninety1 2:01ca44dd3908 20 static float v[3]={0,0,0};
sunninety1 2:01ca44dd3908 21
sunninety1 2:01ca44dd3908 22 float PI = 3.14159265358979323846f;
sunninety1 2:01ca44dd3908 23 void MPUcalibate(){
sunninety1 2:01ca44dd3908 24 static float axx=0,ayy=0,azz=0,gxx=0,gyy=0,gzz=0;
sunninety1 2:01ca44dd3908 25 int x=0;
sunninety1 2:01ca44dd3908 26 while(x<=9999) {
sunninety1 2:01ca44dd3908 27 mpu.getAccelero(ark);
sunninety1 2:01ca44dd3908 28 mpu.getGyro(gy);
sunninety1 2:01ca44dd3908 29 axx+=ark[0];
sunninety1 2:01ca44dd3908 30 ayy+=ark[1];
sunninety1 2:01ca44dd3908 31 azz+=ark[2];
sunninety1 2:01ca44dd3908 32 gxx+=gy[0];
sunninety1 2:01ca44dd3908 33 gyy+=gy[1];
sunninety1 2:01ca44dd3908 34 gzz+=gy[2];
sunninety1 2:01ca44dd3908 35 x+=1;
sunninety1 2:01ca44dd3908 36 wait(0.0001);
sunninety1 2:01ca44dd3908 37 }
sunninety1 2:01ca44dd3908 38 cali_ax=axx/10000.0f;
sunninety1 2:01ca44dd3908 39 cali_ay=ayy/10000.0f;
sunninety1 2:01ca44dd3908 40 cali_az=azz/10000.0f;
sunninety1 2:01ca44dd3908 41 cali_gx=gxx/10000.0f;
sunninety1 2:01ca44dd3908 42 cali_gy=gyy/10000.0f;
sunninety1 2:01ca44dd3908 43 cali_gz=gzz/10000.0f;
sunninety1 2:01ca44dd3908 44 //pc.printf("%.10f %.10f %.10f\n",cali_ax,cali_ay,cali_az-9.80665);
sunninety1 2:01ca44dd3908 45 //pc.printf("%.10f %.10f %.10f\n",cali_ax-9.80665,cali_ay,cali_az);
sunninety1 2:01ca44dd3908 46 //pc.printf("%.10f %.10f %.10f\n",cali_ax,cali_ay-9.80665,cali_az);
sunninety1 2:01ca44dd3908 47 //pc.printf("%f %f %f\n",cali_gx,cali_gy,cali_gz);
sunninety1 2:01ca44dd3908 48 }
sunninety1 2:01ca44dd3908 49 void AccelToVelo(float ax,float ay,float az,float dt){
sunninety1 2:01ca44dd3908 50 v[0]+=(ax*dt/129); //(-156.10655212402344+150.74868774414062);
sunninety1 2:01ca44dd3908 51 v[1]+=(ay*dt/180); //(-208.04095458984375+194.97883605957031);
sunninety1 2:01ca44dd3908 52 v[2]+=(az*dt/6); //(151.52879333496094-144.20004272460938);
sunninety1 2:01ca44dd3908 53 }
sunninety1 2:01ca44dd3908 54 /*float CalculateAngle(float ax,float ay,float az,float gx,float gy,float gz){
sunninety1 2:01ca44dd3908 55 float pitch;
sunninety1 2:01ca44dd3908 56 gx=0;
sunninety1 2:01ca44dd3908 57 gy=0;
sunninety1 2:01ca44dd3908 58 gz=0;
sunninety1 2:01ca44dd3908 59 static float fillter_pitch=0;
sunninety1 2:01ca44dd3908 60 fillter_pitch=(fillter_pitch*0.8)+(pitch*0.2);
sunninety1 2:01ca44dd3908 61 pitch=atan((ax)/sqrt((ay)*(ay)+(az)*(az))*(180/PI));
sunninety1 2:01ca44dd3908 62 //pc.printf("%f\n",pitch);
sunninety1 2:01ca44dd3908 63 return fillter_pitch;
sunninety1 2:01ca44dd3908 64 }*/
sunninety1 2:01ca44dd3908 65 int main()
sunninety1 2:01ca44dd3908 66 {
sunninety1 2:01ca44dd3908 67 pc.baud(115200);
sunninety1 2:01ca44dd3908 68 static float calibate_acc[3]={0.6465099045, 0.14739338841, -0.2634094528};
sunninety1 2:01ca44dd3908 69 static float calibate_gy[3]={-0.022125, 0.005254 ,-0.009761};
sunninety1 2:01ca44dd3908 70 mpu.getAccelero(ark);
sunninety1 2:01ca44dd3908 71 mpu.getGyro(gy);
sunninety1 2:01ca44dd3908 72 //CalculateAngle(ark[0],ark[1],ark[2],0,0,0);
sunninety1 2:01ca44dd3908 73 static float fillter_pitch=0;
sunninety1 2:01ca44dd3908 74 int co=0;
sunninety1 2:01ca44dd3908 75 float pitch;
sunninety1 2:01ca44dd3908 76 float delay=0.0001,current=0.0,deltat;
sunninety1 2:01ca44dd3908 77 t.start();
sunninety1 2:01ca44dd3908 78
sunninety1 2:01ca44dd3908 79 wait(0.5);
sunninety1 2:01ca44dd3908 80
sunninety1 2:01ca44dd3908 81 //MPUcalibate();
sunninety1 2:01ca44dd3908 82 //cali_err=atan((-(ark[1]-cali_ay))/(ark[2]-cali_az));
sunninety1 2:01ca44dd3908 83 //cali_p=atan2(((ark[2]-cali_az)),(ark[0]-cali_ax));
sunninety1 2:01ca44dd3908 84 //cali_p=atan((ark[0]-cali_ax)/sqrt((ark[1]-cali_ay)*(ark[1]-cali_ay)+(ark[2]-cali_az)*(ark[2]-cali_az)));
sunninety1 2:01ca44dd3908 85 //pc.printf("cali P %f \n",cali_err);
sunninety1 2:01ca44dd3908 86 //wait(1);
sunninety1 2:01ca44dd3908 87 /* fillter_pitch2=(fillter_pitch2*0.75)+(fillter_pitch*0.25);
sunninety1 2:01ca44dd3908 88 fillter_pitch3=(fillter_pitch3*0.75)+(fillter_pitch2*0.25);
sunninety1 2:01ca44dd3908 89 fillter_pitch4=(fillter_pitch4*0.75)+(fillter_pitch3*0.25);
sunninety1 2:01ca44dd3908 90 fillter_pitch5=(fillter_pitch5*0.75)+(fillter_pitch4*0.25);
sunninety1 2:01ca44dd3908 91 fillter_pitch6=(fillter_pitch6*0.75)+(fillter_pitch5*0.25);
sunninety1 2:01ca44dd3908 92 fillter_pitch7=(fillter_pitch7*0.75)+(fillter_pitch6*0.25);
sunninety1 2:01ca44dd3908 93 fillter_pitch8=(fillter_pitch8*0.75)+(fillter_pitch7*0.25);
sunninety1 2:01ca44dd3908 94 fillter_pitch9=(fillter_pitch9*0.75)+(fillter_pitch8*0.25);
sunninety1 2:01ca44dd3908 95 fillter_pitch10=(fillter_pitch10*0.75)+(fillter_pitch9*0.25);*/
sunninety1 2:01ca44dd3908 96 while(1) {
sunninety1 2:01ca44dd3908 97 Now = t.read();
sunninety1 2:01ca44dd3908 98 deltat=Now-current;
sunninety1 2:01ca44dd3908 99 if (deltat>0.1){
sunninety1 2:01ca44dd3908 100 mpu.getAccelero(ark);
sunninety1 2:01ca44dd3908 101 mpu.getGyro(gy);
sunninety1 2:01ca44dd3908 102 ark[0] -= calibate_acc[0];
sunninety1 2:01ca44dd3908 103 ark[1] -= calibate_acc[1];
sunninety1 2:01ca44dd3908 104 ark[2] -= calibate_acc[2];
sunninety1 2:01ca44dd3908 105 gy[0] -= calibate_gy[0];
sunninety1 2:01ca44dd3908 106 gy[1] -= calibate_gy[1];
sunninety1 2:01ca44dd3908 107 gy[2] -= calibate_gy[2];
sunninety1 2:01ca44dd3908 108 float g=sqrt((ark[0]*ark[0])+(ark[1]*ark[1])+(ark[2]*ark[2]));
sunninety1 2:01ca44dd3908 109 while(co<1000){
sunninety1 2:01ca44dd3908 110 AccelToVelo((ark[0]),ark[1],ark[2]-9.8065,deltat);
sunninety1 2:01ca44dd3908 111 co+=1;
sunninety1 2:01ca44dd3908 112 }
sunninety1 2:01ca44dd3908 113 co=0;
sunninety1 2:01ca44dd3908 114 ark[0]+=0.5;
sunninety1 2:01ca44dd3908 115 float gra=sqrt((ark[0]*ark[0])+(ark[1]*ark[1])+(ark[2]*ark[2]));
sunninety1 2:01ca44dd3908 116 pc.printf("Q AX %f AY %f AZ %f G %f \n",ark[0],ark[1],ark[2],gra);
sunninety1 2:01ca44dd3908 117 if(serial.writeable()) {
sunninety1 2:01ca44dd3908 118 serial.printf("%f %f %f %f \n",ark[0],ark[1],ark[2],gra);
sunninety1 2:01ca44dd3908 119 //serial.printf("------------------------ \n");
sunninety1 2:01ca44dd3908 120 //serial.printf(" Gyx:%f_Gyy:%f_Gyz:%f \n",gy[0],gy[1],gy[2]);
sunninety1 2:01ca44dd3908 121 wait_ms(500); // lag to debounce the button
sunninety1 2:01ca44dd3908 122 }
sunninety1 2:01ca44dd3908 123 //pc.printf("Q GyX %f GyY %f GyZ %f \n",gy[0],gy[1],gy[2]);
sunninety1 2:01ca44dd3908 124 //pc.printf("Q Vx %f Vy %f Vz %f \n",v[0],v[1],v[2]);
sunninety1 2:01ca44dd3908 125 //pc.printf("%.14f\n",v[1]/100);
sunninety1 2:01ca44dd3908 126 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]);
sunninety1 2:01ca44dd3908 127 //timestamp+=1;
sunninety1 2:01ca44dd3908 128 //pc.printf("%f\n",atan((-(ark[1]-cali_ay))/(ark[2]-cali_az)));
sunninety1 2:01ca44dd3908 129 //pc.printf("Ax %f\n",(atan2((ark[2]-cali_az),(ark[0]-cali_ax))-cali_p)*(180/PI));
sunninety1 2:01ca44dd3908 130 //pitch=atan((ark[0])/sqrt((ark[1])*(ark[1])+(ark[2])*ark[2]))*(180/PI);
sunninety1 2:01ca44dd3908 131 //CalculateAngle(ark[0],ark[1],ark[2],0,0,0);
sunninety1 2:01ca44dd3908 132 //pc.printf("%lf\n",CalculateAngle(ark[0],ark[1],ark[2],0,0,0));
sunninety1 2:01ca44dd3908 133 pitch=atan(ark[0]/sqrt(pow(ark[1],2)+pow(ark[2],2)))*(180/PI);
sunninety1 2:01ca44dd3908 134 //fillter_pitch=(fillter_pitch*0.75)+(pitch*0.25);
sunninety1 2:01ca44dd3908 135 // fillter_pitch2=(fillter_pitch2*0.75)+(fillter_pitch*0.25);
sunninety1 2:01ca44dd3908 136 // pc.printf("%f\n",fillter_pitch);
sunninety1 2:01ca44dd3908 137 //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));
sunninety1 2:01ca44dd3908 138 // pc.printf("%.10f %.10f %.10f\n",ark[0],ark[1],ark[2]);
sunninety1 2:01ca44dd3908 139 //pc.printf("%f %f %f\n",gy[0],gy[1],gy[2]);
sunninety1 2:01ca44dd3908 140 //pc.printf("%.2f\n",t.read());
sunninety1 2:01ca44dd3908 141 wait(0.02);
sunninety1 2:01ca44dd3908 142 /*if (res==0) {
sunninety1 2:01ca44dd3908 143 //lastupdate=t.read()-Now;
sunninety1 2:01ca44dd3908 144 //Now=lastupdate;
sunninety1 2:01ca44dd3908 145 t.reset();
sunninety1 2:01ca44dd3908 146 }*/
sunninety1 2:01ca44dd3908 147 current=Now;
sunninety1 2:01ca44dd3908 148 }
sunninety1 2:01ca44dd3908 149 }
sunninety1 2:01ca44dd3908 150 }