Diff: stateMachines.cpp

Revision:

40:b9450d753782

Parent:

39:de3638276b7e

Child:

41:4dd36f607279

--- a/stateMachines.cpp	Wed Sep 19 12:56:05 2018 +0000
+++ b/stateMachines.cpp	Wed Sep 19 13:13:37 2018 +0000
@@ -348,13 +348,14 @@
     p_section1.nextSection =NULL;// &p_section2;
     p_section1.consigne_position = 75.0;
     p_section1.targetSpeed_cmps = 400.0;
+    p_section1.targetSpeed_cmps = 500.0;
     p_section1.slowSpeed_cmps = 328.0;
     p_section1.coef_p_speed = 1;
     p_section1.lidarWarningDist_cm = 120.0;
-    p_section1.lng_section_cm = 30000.0;//30m
-    p_section1.coef_p = 4;
-    p_section1.coef_i = 0.00001;
-    p_section1.coef_d = 0.00001;
+    p_section1.lng_section_cm = 2000.0;//20m
+    p_section1.coef_p = 0.40;
+    p_section1.coef_d = 0.05;
+    p_section1.coef_i = 0.001;
 
     //section de test
     p_section2.nextSection = NULL;
@@ -651,7 +652,7 @@
     pc.printf("largeurPiste90 => %.4lf \r\n  ",largeurPiste90);*/
 #endif
 
-    //integral correction
+        //integral correction
     lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90);
     integralSum=0;
     for(int f=0; f<NB_INTEGRAL_SAMPLES; f++) {
@@ -659,20 +660,22 @@
     }
     lastDifferenceIndex = (lastDifferenceIndex + 1)%NB_INTEGRAL_SAMPLES;
 
-
-
+     
+    
+    
     //application des coefficients
-    pulseDirection_us_temp = ((positionSurPiste45 - (largeurPiste45/2.0)) * 4)
+    pulseDirection_us_temp = (
+                                ((positionSurPiste45 - (largeurPiste45/2.0)) * p_sectionCourante->coef_p)
+                              + ((positionSurPiste90 - (largeurPiste90/2.0)) * p_sectionCourante->coef_p)
+                              + ( -derive45                                  * p_sectionCourante->coef_d)
+                              + ( -derive90                                  * p_sectionCourante->coef_d)
+                              + ( integralSum                                * p_sectionCourante->coef_i)
+                             )* (300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul
                             + DIRECTION_PULSE_MIDDLE;
-    /*pulseDirection_us_temp = COEF_RAYON_BR_S_DIST/tachySpeed_cmps * //facteur de vitesse: plus on va vite, moins on corrige
-                        (
-                          ( (p_sectionCourante->consigne_position - positionSurPiste45) * p_sectionCourante->coef_p)
-                        //+ ( derive45 *  p_sectionCourante->coef_d)
-                        //+ ( integralSum * p_sectionCourante->coef_i)
-                        )
-                        + DIRECTION_PULSE_MIDDLE;*/
-                        //pc.printf("\r %.4lf   ",pulseDirection_us);
+                            
     pulseDirection_us = (int)pulseDirection_us_temp ;
+    
+    
 #ifdef OMAR
         pc.printf("p_sectionCourante->consigne_position => %.4lf \r\n  ",p_sectionCourante->consigne_position);
         pc.printf("positionSurPiste45 => %.4lf \r\n  ",positionSurPiste45);