Revision 14:f4cbc5db2873, committed 2020-06-03

Comitter:

gr66

Date:

Wed Jun 03 20:23:57 2020 +0000

Parent:

13:0549a3e57bc4

Commit message:

V1.0

Changed in this revision

diff -r 0549a3e57bc4 -r f4cbc5db2873 main.cpp
--- a/main.cpp	Fri May 29 13:54:52 2020 +0000
+++ b/main.cpp	Wed Jun 03 20:23:57 2020 +0000
@@ -1,4 +1,10 @@
-// test avec RTOS
+// *******************************************************
+// HUBLEX - Université Paris Saclay - IUT de Cachan
+// Version 1.0  du 3 juin 2020
+// Auteur : Gilles Raynaud
+//
+// SteadyTech
+//*********************************************************
 #define viti 1    // 1 montage viti ;  0 montage sur table
 #include "mbed.h"
 #include "LSM9DS1.h"
@@ -14,15 +20,15 @@
 //
 DigitalOut LedVP(PC_8);  // led Verin P
 DigitalOut LedVM(PC_5);  // led Verin M
-DigitalOut LedOK(PC_6);   // led Système OK
+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;
-AnalogIn verin(PC_3);  // lecture position verin
-AnalogOut ana (PA_5);   // pour debug analogique ISR et RTOS
+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;
 //
@@ -32,11 +38,11 @@
 //
 double ang;
 // fitres complémentaires
-double Fc=0.05;
-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 Fc=0.05;  // fréquence charnière du filtre complémentaire
+double RC  ;         //constante de temps
+double a0;    //coefficient a0 du filtre
+double b0;    //coefficient b0 du filtre
+double a1;    //coefficient a1 du filtre
 //
 double angle_acce_pred=0.0f;
 double angle_acce=0.0f;
@@ -55,9 +61,9 @@
     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
-    a1=a0*RC*1.0;           //calcul du coefficient a du filtre
+    a0=1./(1+(2*RC/dt));   //calcul du coefficient a0 du filtre
+    b0=(1-(2.*RC/dt))*a0;    //calcul du coefficient b0 du filtre
+    a1=a0*RC*1.0;           //calcul du coefficient a1 du filtre
     DOF.readAccel();
     DOF.readGyro();
 #if viti
@@ -91,28 +97,17 @@
     LedVP=1;
     LedVM=1;
     LedOK=1;
-    wait(0.5);
+    DOF.begin();
+    for(int ii=0; ii<4; ii++) {
+        DOF.calibration();
+        LedVP=!LedVP;
+        LedVM=!LedVM;
+        LedOK=!LedOK;
+    }
     LedVP=0;
     LedVM=0;
-    LedOK=0;
-    wait(0.5);
-    LedVP=1;
-    LedVM=1;
     LedOK=1;
-    DOF.begin();
-    wait(0.2);
-    LedVP=0;
-    LedVM=0;
-    LedOK=0;
-    DOF.calibration();
-    LedVP=1;
-    LedVM=1;
-    LedOK=1;
-    wait(0.5);
-    LedVP=0;
-    LedVM=0;
-    LedOK=0;
-    wait(0.5);
+    //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