code projet M1 + ensil
Dependencies: ADXL345 HMC5843 ITG3200 mbed
Fork of 9DOF-Stick by
Diff: main.cpp
- Revision:
- 2:c340f1bda440
- Parent:
- 1:18f68a0b0793
diff -r 18f68a0b0793 -r c340f1bda440 main.cpp --- a/main.cpp Fri Oct 07 21:42:29 2011 +0000 +++ b/main.cpp Wed Mar 28 17:01:42 2018 +0000 @@ -2,16 +2,51 @@ #include "HMC5843.h" #include "ADXL345.h" #include "ITG3200.h" +#include <math.h> + +#define PI 3.14159265358979323846f DigitalOut myled(LED1); -HMC5843 cmp(p28, p27); // sda, scl -ADXL345 acc(p28, p27); // sda, scl -ITG3200 gyr(p28, p27); // sda, scl -Serial pc(USBTX, USBRX); // tx, rx +HMC5843 cmp(D4, D5); // sda, scl +ADXL345 acc(D4, D5); // sda, scl +ITG3200 gyr(D4, D5); // sda, scl + +Serial pc(SERIAL_TX, SERIAL_RX); // tx, rx +Serial BT(PA_9, PA_10); // tx, rx + +Timer t; + +float acc_x,acc_y,acc_z; +float mag_x,mag_y,mag_z; +float gyr_x,gyr_y,gyr_z; + +float theta,phi,psi; +float theta1,phi1,psi1; + +double Last, Now; +bool START = true; +int count; + + float x_x_filter[3]={0,0,0}, x_y_filter[3]={0,0,0}; + float y_x_filter[3]={0,0,0}, y_y_filter[3]={0,0,0}; + float z_x_filter[3]={0,0,0}, z_y_filter[3]={0,0,0}; + float a_coef[3]={1.0000, -0.3695, 0.1958}; //coefs pour filtre PB IIR2 à fréquence coupure normalisé 0.4 + float b_coef[3]={0.2066, 0.4131, 0.2066}; + float theta_filtre, phi_filtre, psi_filtre; + + float matrice[3][3], resultat[3]; + float ca,cb,cc,sa,sb,sc; + float tableau_x[10], tableau_y[10], tableau_z[10]; + int main() { - pc.baud(115200); + pc.baud(9600); + BT.baud(9600); + + pc.printf("hello word\n"); + BT.printf("connection...\n"); + //values x,y,z int readings[3]; //ID Buffer @@ -60,26 +95,196 @@ pc.printf("%c" ,13,10); wait(1); + + t.start(); + Last = t.read_us(); + count=0; while (1) { + Now = t.read_us(); + float delta = (float) (Now-Last)/1000000.0f; + if (delta>=0.1f && START==true) { + + pc.printf("delta = %f (en s)\n",delta); + + Last=Now; + count+=1; cmp.readData(readings); - pc.printf("C %+4i %+4i %+4i",(int16_t)readings[0],(int16_t)readings[1],(int16_t)readings[2]); - wait(0.05); + mag_x=((float)(int16_t)readings[0])*(0.92f/1000.0f); //valeur au nord de 470mGa (miliGauss) + mag_y=((float)(int16_t)readings[1])*(0.92f/1000.0f); + mag_z=((float)(int16_t)readings[2])*(0.92f/1000.0f); + pc.printf("C %+f %+f %+f (en Ga)",mag_x,mag_y,mag_z); + //wait(0.05); acc.getOutput(readings); - pc.printf(" A %+5i %+5i %+5i",(int16_t)readings[0],(int16_t)readings[1],(int16_t)readings[2]); - wait(0.05); + acc_x=-((float)(int16_t)readings[0])/266.0f; + acc_y=-((float)(int16_t)readings[1])/256.0f; + acc_z=-((float)(int16_t)readings[2])/256.0f-0.1f; + pc.printf(" A %+f %+f %+f (en g)",acc_x,acc_y,acc_z); + //wait(0.05); - pc.printf(" G %+5i %+5i %+5i",(int16_t)gyr.getGyroX(),(int16_t)gyr.getGyroY(),(int16_t)gyr.getGyroZ()); + gyr_x=(((float)(int16_t)gyr.getGyroX())/14.375f)+4.4f; + gyr_y=(((float)(int16_t)gyr.getGyroY())/14.375f); + gyr_z=(((float)(int16_t)gyr.getGyroZ())/14.375f)-1.6f; + pc.printf(" G %+f %+f %+f (en deg/s)",gyr_x,gyr_y,gyr_z); pc.printf("%c" ,13,10); - wait(0.05); + //wait(0.05); + + float signe_z=1.0f; + if (acc_z<0) { signe_z=-1.0f; } - if (myled) { - myled=0; - } else { - myled=1; + //Theta = Roulis (en X) + if (abs(theta1*180.0f/PI)<80.0f){ + theta = atan2(acc_y,sqrt(acc_x*acc_x+acc_z*acc_z)) ; + } + else { + theta = atan2(acc_y,signe_z*sqrt(acc_x*acc_x+acc_z*acc_z)) ; + } + //Phi = Tangage (en Y) + if (abs(psi1*180.0f/PI)<80.0f){ + phi = atan2(acc_x,sqrt(acc_y*acc_y+acc_z*acc_z)) ; + } + else { + phi = atan2(acc_x,signe_z*sqrt(acc_y*acc_y+acc_z*acc_z)) ; + } + //Psi = Lacet (en Z) + //psi = atan2( (-mag_y*cos(phi) + mag_z*sin(phi) ) , (mag_x*cos(theta) + mag_y*sin(theta)*sin(phi)+ mag_z*sin(theta)*cos(phi)) ) ; + float signe_z_mag=1.0f; + if (mag_z<0) { signe_z_mag=-1.0f; } + if(mag_y>0) + { + psi = signe_z_mag*(PI/2.0f-(atan(mag_x/mag_y))); + } + else if (mag_y<0) + { + psi = signe_z_mag*(-PI/2.0f-(atan(mag_x/mag_y))); + } + else if (mag_y==0 && mag_x<0) + { + psi = PI; + } + else + { + psi = 0; + } + if(psi>=0){ + psi=(psi-50.0f*PI/180.0f)*(180.0f/64.0f); + } + else { + psi=(psi+50.0f*PI/180.0f)*(180.0f/82.0f); } + + theta1=theta; + phi1=phi; + psi1=psi; + + pc.printf("angles %+f %+f %+f (en deg)\n",theta*180.0f/PI,phi*180.0f/PI,psi*180.0f/PI); + + + //Filtrage passe bas + int N=2; //ordre du filtre + for (int i=0;i<N;i++){ + x_x_filter[N-i]=x_x_filter[N-i-1]; + y_x_filter[N-i]=y_x_filter[N-i-1]; + z_x_filter[N-i]=z_x_filter[N-i-1]; + } + x_x_filter[0]=theta; + y_x_filter[0]=phi; + z_x_filter[0]=psi; + for (int i=0;i<N;i++){ + x_y_filter[N-i]=x_y_filter[N-i-1]; + y_y_filter[N-i]=y_y_filter[N-i-1]; + z_y_filter[N-i]=z_y_filter[N-i-1]; + } + x_y_filter[0]=b_coef[0]*x_x_filter[0]; + y_y_filter[0]=b_coef[0]*y_x_filter[0]; + z_y_filter[0]=b_coef[0]*z_x_filter[0]; + for (int i=1;i<=N;i++){ + x_y_filter[0]+=b_coef[i]*x_x_filter[i]-a_coef[i]*x_y_filter[i]; + y_y_filter[0]+=b_coef[i]*y_x_filter[i]-a_coef[i]*y_y_filter[i]; + z_y_filter[0]+=b_coef[i]*z_x_filter[i]-a_coef[i]*z_y_filter[i]; + } + theta_filtre=x_y_filter[0]; + phi_filtre=y_y_filter[0]; + psi_filtre=z_y_filter[0]; + //theta_filtre=theta; + //phi_filtre=phi; + //psi_filtre=psi; + + pc.printf("angles filtres %+f %+f %+f (en deg)\n",theta_filtre*180.0f/PI,phi_filtre*180.0f/PI,psi_filtre*180.0f/PI); + + ///matrice d'Euler + ca=cos(theta_filtre); cb=cos(phi_filtre); cc=cos(psi_filtre); + sa=sin(theta_filtre); sb=sin(phi_filtre); sc=sin(psi_filtre); + + matrice[0][0] = cc*cb; + matrice[0][1] = -sc*ca + cc*sb*sa; + matrice[0][2] = sc*sa + cc*sb*ca; + matrice[1][0] = sc*cb; + matrice[1][1] = cc*ca + sc*sb*sa; + matrice[1][2] = -cc*sa + sc*sb*ca; + matrice[2][0] = -sb; + matrice[2][1] = cb*sa; + matrice[2][2] = cb*ca; + + for(int i=0; i<3; i++) + { + float temp = 0; + temp = acc_x * matrice[i][0] + acc_y * matrice[i][1] + acc_z * matrice[i][2]; + resultat[i] = temp; + } + + float poids = -1.0f; + if(resultat[2]<0){ + poids = 1.0f; + } + + pc.printf("apres Euler : %+f %+f %+f (en g)\n",resultat[0],resultat[1],resultat[2]+poids); + + tableau_x[count-1]=resultat[0]; + tableau_y[count-1]=resultat[1]; + tableau_z[count-1]=resultat[2]+poids; + + if (count>=10){ + myled=!myled; + count=0; + float out_x =0; + float out_y =0; + float out_z =0; + for(int i=0;i<10;i++){ + out_x+=tableau_x[i]; + out_y+=tableau_y[i]; + out_z+=tableau_z[i]; + //BT.printf("%d;%d ",(int32_t)(tableau_x[i]*1000.0f),(int32_t)(tableau_y[i]*1000.0f)); + } + out_x/=10.0f; + out_y/=10.0f; + out_z/=10.0f; + +///modifier ici + BT.printf("%f",0.25f); //out_x); //,(int8_t)(out_y*1000.0f)); + pc.printf("\n%f ; %f\n\n",out_x,out_y); + + START=false; + + //BT.printf("E%f,%f,%f\n",out_x,out_y,out_z); + + } + } + + if (BT.readable()) { + char c = BT.getc(); + if(c == '1') { + //BT.printf("\nOK\n"); + START=true; + } + if(c == '0') { + //BT.printf("\nOK\n"); + START=false; + } + } + } }