![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
V1
main.cpp
- Committer:
- gr66
- Date:
- 2020-05-21
- Revision:
- 1:a524fa7b2606
- Parent:
- 0:e5a92da6a6fe
File content as of revision 1:a524fa7b2606:
#include "mbed.h" #include "LSM9DS1.h" #include "TB6549.h" #define dt 0.01 // pas d'integration DigitalOut Led0(LED1); Serial pc(SERIAL_TX, SERIAL_RX,115200); Ticker calc; Ticker affich; LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38); AnalogIn verin(PC_3); // lecture pos verin AnalogOut ana (PA_5); // pour debug analogique ISR Motor motor(PB_4,PA_1,PA_4,PC_7); int flag_affich=0; int flag_imu=0; double pi= 3.1415926535897932; double gx_off=0,gy_off=0,gz_off=0; double ang_off=0; //void gyro_zero(void); //void angle_zero(void); void calcule() { flag_imu=1; } void affiche() { flag_affich=1; } int main() { double ang; // fitres complémentaires double Fc=0.05; double RC=1./(Fc*2*pi); //calcul de RC double a0=1./(1+(2*RC/dt)); //calcul du coefficient a du filtre double b0=(1-(2.*RC/dt))*a0; //calcul du coefficient b du filtre double a1=a0*RC*1.0; //calcul du coefficient a du filtre double angle_acce_pred=0.0f; double angle_acce=0.0f; double angle_acce_f_pred=0.0f; double angle_acce_f=0.0f; // double gyroy_pred=0.0f; double gyroy=0.0f; double angle_gyroy_f_pred=0.0f; double angle_gyroy_f=0.0f; // double angle_final; wait(1); //DOF.calibration(); DOF.begin(); wait(1); DOF.calibration(); wait(1); calc.attach(&calcule,dt); affich.attach(&affiche,0.1); while(1) { if(flag_imu) { ana=0.6; DOF.readAccel(); DOF.readGyro(); ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off); // sur table //ang=((180/pi)*atan2((double)DOF.ax,(double)DOF.ay)-ang_off); // sur site // filtres complémentaires angle_acce_pred = angle_acce; angle_acce=ang; angle_acce_f_pred = angle_acce_f; angle_acce_f=a0*angle_acce+a0*angle_acce_pred-b0*angle_acce_f_pred; //filtrage accéléromètre gyroy_pred = gyroy; gyroy=-DOF.gx-gx_off; //sur table // gyroy=-DOF.gz-gz_off; //sur site angle_gyroy_f_pred = angle_gyroy_f; angle_gyroy_f=a1*gyroy+a1*gyroy_pred-b0*angle_gyroy_f_pred; // angle_final=angle_acce_f+angle_gyroy_f; // // flag_imu=0; ana=0.0; } if(flag_affich) { ana=0.3; //moteur float x=verin.read(); float s=0.0; if((angle_final>2)&&(x>0.1)) s=0.4; else if((angle_final<-2)&&(x<0.9)) s=-0.4; else motor.speed(0.0); motor.speed(s); pc.printf("$%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f;\r\n",ang,angle_acce_f,angle_gyroy_f,angle_final,gyroy,x,s); ana=0.0; flag_affich=0; } } } void gyro_zero(void) { const int NN=1000; //float GyroBuffer[3]; for(int i=0; i<NN; i++) { DOF.readGyro(); gx_off=gx_off+DOF.gx/(NN); gy_off=gy_off+DOF.gy/(NN); gz_off=gz_off+DOF.gz/(NN); } } void angle_zero(void) { const int NN=1000; //int16_t AccBuffer[3]; for(int i=0; i<NN; i++) { DOF.readAccel(); double ang=(180/pi)*atan2((double)DOF.ay,(double)DOF.az); // sur table //double ang=(180/pi)*atan2((double)DOF.ax,(double)DOF.ay); // sur site ang_off=ang_off+ang/NN; } }