code projet M1 + ensil
Dependencies: ADXL345 HMC5843 ITG3200 mbed
Fork of 9DOF-Stick by
main.cpp
- Committer:
- JohanBeverini
- Date:
- 2018-03-28
- Revision:
- 3:15f5e8de8533
- Parent:
- 2:c340f1bda440
File content as of revision 3:15f5e8de8533:
#include "mbed.h" #include "HMC5843.h" #include "ADXL345.h" #include "ITG3200.h" #include <math.h> #define PI 3.14159265358979323846f DigitalOut myled(LED1); 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(9600); BT.baud(9600); pc.printf("hello word\n"); BT.printf("connection...\n"); //values x,y,z int readings[3]; //ID Buffer char buffer[3]; pc.printf("%c" ,13,10,13,10,13,10); // do init stuff //Continuous mode, , 10Hz measurement rate. // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA cmp.setDefault(); //Wait some time(Need at least 5ms) wait(0.1); cmp.getAddress(buffer); pc.printf("cmp Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]); // These are here to test whether any of the initialization fails. It will print the failure if (acc.setPowerControl(0x00)){ pc.printf("acc: didn't intitialize power control\n"); return 0; } wait(.001); //Full resolution, +/-16g, 4mg/LSB. if(acc.setDataFormatControl(0x0B)){ pc.printf("didn't set data format\n"); return 0; } wait(.001); //3.2kHz data rate. if(acc.setDataRate(ADXL345_3200HZ)){ pc.printf("didn't set data rate\n"); return 0; } wait(.001); //Measurement mode. if(acc.setPowerControl(MeasurementMode)) { pc.printf("didn't set the power control to measurement\n"); return 0; } pc.printf("Acc Id=%x \n\r", acc.getDeviceID()); pc.printf("%c" ,13,10); //Set highest bandwidth. gyr.setLpBandwidth(LPFBW_256HZ); pc.printf("Gyro Id=%x \n\r", gyr.getWhoAmI()); 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); 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); 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); 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); float signe_z=1.0f; if (acc_z<0) { signe_z=-1.0f; } //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; } } } }