Diff: main.cpp

Revision:

13:0549a3e57bc4

Parent:

12:ab387ced85ab

Child:

14:f4cbc5db2873

--- a/main.cpp	Tue May 26 13:56:30 2020 +0000
+++ b/main.cpp	Fri May 29 13:54:52 2020 +0000
@@ -2,36 +2,30 @@
 #define viti 1    // 1 montage viti ;  0 montage sur table
 #include "mbed.h"
 #include "LSM9DS1.h"
-
-
-
 #include "TB6549.h"
-
+//
 #define dt 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)
-
+#define ZM 2.0             // zone morte +/- zm
+#define SP 0.6             // pwm moteur en % (0-1)
+//
 void calcul(void);
-
+//
 DigitalOut LedVP(PC_8);  // led Verin P
 DigitalOut LedVM(PC_5);  // led Verin M
 DigitalOut LedOK(PC_6);   // led Système OK
-
+//
 Serial pc(SERIAL_TX, SERIAL_RX,115200);
-
+//
 LSM9DS1 DOF(PB_9, PB_8, 0xD4, 0x38);
 Thread thread (osPriorityAboveNormal);
 EventQueue queue;
-//Mutex pr;
-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);
+AnalogIn verin(PC_3);  // lecture position verin
+AnalogOut ana (PA_5);   // pour debug analogique ISR et RTOS
+Motor motor(PB_4,PA_1,PA_4,PC_7);  // commande moteur vérin
 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;
@@ -43,7 +37,7 @@
 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;
 double angle_acce_f_pred=0.0f;
@@ -58,8 +52,8 @@
 //
 void calcul(void)
 {
-    ana=0.3;
-    Fc=0.05;
+    ana=0.3;  // debug RTOS
+    Fc=0.05;    // frequence de coupure des filtres
     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
@@ -97,16 +91,16 @@
     LedVP=1;
     LedVM=1;
     LedOK=1;
-    wait(1);
+    wait(0.5);
     LedVP=0;
     LedVM=0;
     LedOK=0;
-    wait(1);
+    wait(0.5);
     LedVP=1;
     LedVM=1;
     LedOK=1;
     DOF.begin();
-    wait(1);
+    wait(0.2);
     LedVP=0;
     LedVM=0;
     LedOK=0;
@@ -114,36 +108,40 @@
     LedVP=1;
     LedVM=1;
     LedOK=1;
-    wait(1);
+    wait(0.5);
     LedVP=0;
     LedVM=0;
     LedOK=0;
+    wait(0.5);
+    // init filtre accéléro
+    DOF.readAccel();
+    angle_acce_f_pred=((180/pi)*atan2((double)DOF.ay,(double)DOF.ax)+90.00-ang_off);  // sur site USB gauche
+    angle_acce_f=((180/pi)*atan2((double)DOF.ay,(double)DOF.ax)+90.00-ang_off);  // sur site USB gauche
+    //
     thread.start(callback(&queue,&EventQueue::dispatch_forever));
     tic.attach(queue.event(&calcul),dt);
     LedOK=1;
     while(1) {
-        ana=0.6;
+        ana=0.6;  // debug RTOS
         //moteur
-        float x=verin.read();
-        float s=0.0;
+        double x=verin.read();
+        double s=0.0;
         if((angle_final<-ZM)&&(x>FDC_MOINS)) {
             s=SP;
             LedVP=1;
             LedVM=0;
         } else if((angle_final>ZM)&&(x<FDC_PLUS)) {
             s=-SP;
-            LedVP=1;
-            LedVM=0;
+            LedVP=0;
+            LedVM=1;
         } else {
             s=0;
             LedVP=0;
             LedVM=0;
         }
-        //
         motor.speed(s);
-        //pr.lock();
         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);
-        //pr.unlock();
+        //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);
         ana=0.0;
         wait(0.095);
     }