Diff: main.cpp

Revision:

3:d552b7419f51

Parent:

2:952d41c26b43

Child:

4:b40027cb3012

--- a/main.cpp	Thu May 21 18:23:34 2020 +0000
+++ b/main.cpp	Thu May 21 19:35:53 2020 +0000
@@ -11,10 +11,10 @@
 DigitalOut Led0(LED1);
 
 Serial pc(SERIAL_TX, SERIAL_RX,115200);
-Ticker calc;
-Ticker affich;
+//Ticker calc;
+//Ticker affich;
 LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
-
+Thread thread;
 
 AnalogIn verin(PC_3);  // lecture pos verin
 AnalogOut ana (PA_5);   // pour debug analogique ISR
@@ -28,85 +28,83 @@
 
 //void gyro_zero(void);
 //void angle_zero(void);
+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
 
-void calcule()
+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;
+void calcul(void)
 {
-    flag_imu=1;
-}
-void affiche()
-{
-    flag_affich=1;
+    while(1) {
+        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;
+
+        //
+        //
+
+        ana=0.0;
+        wait(0.0082);
+    }
 }
 
 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);
+    thread.start(calcul);
+
     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;
-        }
+        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;
+        wait(0.1);
+
     }
 }