Diff: main.cpp

Revision:

8:41752698aa35

Parent:

7:f5c5cf499311

Child:

9:2a16e3930e1d

--- a/main.cpp	Sat May 23 14:18:53 2020 +0000
+++ b/main.cpp	Sat May 23 16:15:40 2020 +0000
@@ -13,28 +13,27 @@
 #define ZM 2               // zone morte +/- zm
 #define SP 0.5             // pwm moteur en % (0-1)
 
-
+void calcul(void);
 DigitalOut Led0(LED1);
 
 Serial pc(SERIAL_TX, SERIAL_RX,115200);
 
 LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
 Thread thread (osPriorityAboveNormal);
+EventQueue queue;
 
 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);
 Timer tt;
+Ticker tic;
 
 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);
+//
 double ang;
 // fitres complémentaires
 double Fc=0.05;
@@ -58,51 +57,44 @@
 //
 void calcul(void)
 {
-    while(1) {
-        ana=0.3;
-        Fc=0.05;
-        RC=1./(Fc*2*pi);            //calcul de RC
-        a0=1./(1+(2*RC/dt));   //calcul du coefficient a du filtre
-        b0=(1-(2.*RC/dt))*a0;    //calcul du coefficient b du filtre
-        a1=a0*RC*1.0;           //calcul du coefficient a du filtre
-        dt=tt.read_us()/1000000.0;
-        tt.reset();
-        DOF.readAccel();
-        DOF.readGyro();
+    ana=0.3;
+    Fc=0.05;
+    RC=1./(Fc*2*pi);            //calcul de RC
+    a0=1./(1+(2*RC/dt));   //calcul du coefficient a du filtre
+    b0=(1-(2.*RC/dt))*a0;    //calcul du coefficient b du filtre
+    a1=a0*RC*1.0;           //calcul du coefficient a du filtre
+    dt=tt.read_us()/1000000.0;
+    tt.reset();
+    DOF.readAccel();
+    DOF.readGyro();
 #if viti
-        ang=((180/pi)*atan2((double)DOF.ax,-(double)DOF.ay)-90-ang_off);  // sur site
+    ang=((180/pi)*atan2((double)DOF.ax,-(double)DOF.ay)-90-ang_off);  // sur site
 #else
-        ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off);       // sur table
+    ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off);       // sur table
 #endif
 
-        // 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
+    // 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_pred = gyroy;
 #if viti
-        gyroy=DOF.gz-gz_off;  //sur site
+    gyroy=DOF.gz-gz_off;  //sur site
 #else
-        gyroy=-DOF.gx-gx_off;  //sur table
+    gyroy=-DOF.gx-gx_off;  //sur table
 #endif
 
-        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;
-ana=0.0;
-        //
-        //
-
-        wait(0.0083);
-    }
+    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;
+    ana=0.0;
 }
 
 int main()
 {
-
     dt=dt0;
     tt.start();
     wait(1);
@@ -112,8 +104,10 @@
     DOF.calibration();
     wait(1);
     tt.reset();
-    thread.start(calcul);
-    
+    thread.start(callback(&queue,&EventQueue::dispatch_forever));
+    tic.attach(queue.event(calcul),0.01);
+
+
 
     while(1) {
 
@@ -136,6 +130,7 @@
         motor.speed(s);
         pc.printf("$%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f %f;\r\n",ang,angle_acce_f,angle_gyroy_f,angle_final,gyroy,x,s,dt);
         ana=0.0;
+
         wait(0.095);
 
     }