Diff: main.cpp

Revision:

5:d51ba8e93d10

Parent:

4:b40027cb3012

Child:

6:65c511f6f1fc

--- a/main.cpp	Fri May 22 14:19:42 2020 +0000
+++ b/main.cpp	Sat May 23 10:02:24 2020 +0000
@@ -1,4 +1,5 @@
 // test avec RTOS
+#define viti 1    // 1 montage viti ;  0 montage sur table
 #include "mbed.h"
 #include "LSM9DS1.h"
 
@@ -6,19 +7,25 @@
 
 #include "TB6549.h"
 
-#define dt 0.01     // pas d'integration
+#define dt0 0.01            // pas d'integration
+#define FDC_PLUS 0.9       // fin de course +
+#define FDC_MOINS 0.1      // din de course -
+#define ZM 2               // zone morte +/- zm
+#define SP 0.5             // pwm moteur en % (0-1)
+
 
 DigitalOut Led0(LED1);
 
 Serial pc(SERIAL_TX, SERIAL_RX,115200);
-//Ticker calc;
-//Ticker affich;
+
 LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
 Thread thread (osPriorityAboveNormal);
 
 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;
+
 int flag_affich=0;
 int flag_imu=0;
 double pi= 3.1415926535897932;
@@ -31,10 +38,10 @@
 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 RC  ;         //calcul de RC
+double a0;   //calcul du coefficient a du filtre
+double b0;    //calcul du coefficient b du filtre
+double a1;           //calcul du coefficient a du filtre
 
 double angle_acce_pred=0.0f;
 double angle_acce=0.0f;
@@ -46,25 +53,41 @@
 double angle_gyroy_f_pred=0.0f;
 double angle_gyroy_f=0.0f;
 //
+double dt=dt0 ; // pas reel
 double angle_final;
 void calcul(void)
 {
     while(1) {
+        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();
+        tt.start();
         ana=0.6;
         DOF.readAccel();
         DOF.readGyro();
+        #if viti
+        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.ax,(double)DOF.ay)-ang_off);  // sur site
+        #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
-
+        
         gyroy_pred = gyroy;
+        #if viti
+        gyroy=DOF.gz-gz_off;  //sur site
+        #else
         gyroy=-DOF.gx-gx_off;  //sur table
-        // gyroy=-DOF.gz-gz_off;  //sur site
+        #endif       
+        
         angle_gyroy_f_pred = angle_gyroy_f;
         angle_gyroy_f=a1*gyroy+a1*gyroy_pred-b0*angle_gyroy_f_pred;
         //
@@ -81,7 +104,7 @@
 int main()
 {
 
-
+dt=dt0;
     wait(1);
     //DOF.calibration();
     DOF.begin();
@@ -97,9 +120,17 @@
         //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;
+        /*
+        // sur table
+        if((angle_final>2)&&(x>0.1)) s=0.6;
+        else if((angle_final<-2)&&(x<0.9)) s=-0.6;
         else motor.speed(0.0);
+        */
+        //sur site
+        if((angle_final<-ZM)&&(x>FDC_MOINS)) s=SP;
+        else if((angle_final>ZM)&&(x<FDC_PLUS)) s=-SP;
+        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;
@@ -108,25 +139,3 @@
     }
 }
 
-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;
-    }
-}
\ No newline at end of file