Diff: stateMachines.cpp

Revision:

47:b17061738568

Parent:

46:9262b07b0833

Child:

50:44c0a075e78d

Child:

51:09ecba68d0cf

--- a/stateMachines.cpp	Thu Sep 20 18:45:07 2018 +0000
+++ b/stateMachines.cpp	Thu Sep 20 20:14:22 2018 +0000
@@ -261,14 +261,15 @@
 
 double getDistMoy(AnalogIn* p,double* tab,int size)
 {
-    tab[index_fifo_ir] =  3.3 * (double)p->read(); // on convertit directement en volts
+    double distcapteur = 0;
+    distcapteur =  1.0/(0.0161 *3.3 * (double)p->read()); // on convertit directement en volts
     //tension proportionelle à l'inverse de la distance
-    tab[index_fifo_ir] = 1.0/(0.0161 * (double)tab[index_fifo_ir]);
-    int sumMoy = 0;
-    for(int k=0; k<size; k++) {
-        sumMoy+=tab[k];
-    }
-    return sumMoy/size;
+    //tab[index_fifo_ir] = 1.0/(0.0161 * (double)tab[index_fifo_ir]);
+    //int sumMoy = 0;
+    //for(int k=0; k<size; k++) {
+    //    sumMoy+=tab[k];
+    //}
+    return distcapteur;
 }
 void tachyCheck()
 {
@@ -365,8 +366,8 @@
     p_section1.coef_p_speed = 1;
     p_section1.lidarWarningDist_cm = 120.0;
     p_section1.lng_section_cm = 1000.0;//10m
-    p_section1.coef_p = 4.0;
-    p_section1.coef_d = 0.2;
+    p_section1.coef_p = 20.0;
+    p_section1.coef_d = -5.0;
     p_section1.coef_i = 0.000;
 
     //section de test
@@ -377,7 +378,7 @@
     p_section2.coef_p_speed = 1.0;
     p_section2.lidarWarningDist_cm = 300.0;
     p_section2.lng_section_cm = 200.0;//2m
-    p_section2.coef_p = 1;
+    p_section2.coef_p = 5.0;
     p_section2.coef_i = 0.00001;
     p_section2.coef_d = 0.00001;
 
@@ -644,7 +645,7 @@
             case REF_A_DROITE://par defaut, on compte à partir de la bordure gauche
                 largeurPiste90 = 150.0;
                 positionSurPiste90 = largeurPiste90 - ( DEMI_LARGEUR_BINIOU_CM + trueDistMurD90Moy ) ;
-                positionSurPiste45 = largeurPiste90 - ( distMurD45Moy * (1.414214)/2 ) + DEMI_LARGEUR_BINIOU_CM;
+                positionSurPiste45 = largeurPiste90 - ( distMurD45Moy * (1.414214)/2.0 ) + DEMI_LARGEUR_BINIOU_CM;
 #ifdef OMAR
                 pc.printf("REF_A_DROITE\r\n");
 #endif
@@ -693,10 +694,10 @@
         pulseDirection_us = (int)(
                                 ((positionSurPiste45 - 75.0) * p_sectionCourante->coef_p)
                                 //+ ((positionSurPiste90 - 75.0) * p_sectionCourante->coef_p)
-                                //+ ( -derive45                                  * p_sectionCourante->coef_d)
+                                + ( -derive45                                  * p_sectionCourante->coef_d)
                                 //+ ( -derive90                                  * p_sectionCourante->coef_d)
                                 //+ ( -integralSum                               * p_sectionCourante->coef_i)
-                            )* 1;//(300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul
+                            )//(300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul
                             + DIRECTION_PULSE_MIDDLE;