Diff: stateMachines.cpp

Revision:

50:44c0a075e78d

Parent:

47:b17061738568

--- a/stateMachines.cpp	Thu Sep 20 20:14:22 2018 +0000
+++ b/stateMachines.cpp	Fri Sep 21 12:53:52 2018 +0000
@@ -138,7 +138,7 @@
         history[m].time = 0;
         history[m].position45 = 0.0;
         history[m].position90 = 0.0;
-        history[m].largeurPiste = 0.0;
+        history[m].largeurPiste90 = 0.0;
         history[m].dist = 0.0;
         history[m].pwm_thro_us = 0;
         history[m].pwm_dir_us = 0;
@@ -169,7 +169,7 @@
             history[indexSample].time = timeSinceStart.read_us() ;
             history[indexSample].position45 = positionSurPiste45;
             history[indexSample].position90 = positionSurPiste90;
-            history[indexSample].largeurPiste = largeurPiste90;
+            history[indexSample].largeurPiste90 = largeurPiste90;
             history[indexSample].pwm_thro_us = pulseSpeed_us;
             history[indexSample].pwm_dir_us = pulseDirection_us;
             history[indexSample].dist = tachySectionDist_cm,
@@ -194,19 +194,19 @@
 #if DEBUG > 0
     pc.printf("[START TO TRANSMIT]\r\n");
 #endif
-    serialLidar.printf("time,position 45,position 90,largeur piste,pwm_thro_us,pwm_dir_us,dist,murs/dlvv,section,maxSpeed,throttle\r\n");
+    serialLidar.printf("time;position 45;position 90;largeur piste;pwm_dir_us;dist;throttle\r\n");
     for(int p=0; p<indexSample; p++) {
-        serialLidar.printf("%d,%.5f,%.5f,%.5f,%d,%d,%.5f,%d,%d,%d,%d\r\n",
+        serialLidar.printf("%d;%.5f;%.5f;%.5f;%d;%d;%d\r\n",
                            history[p].time,
                            history[p].position45,
                            history[p].position90,
-                           history[p].largeurPiste,
-                           history[p].pwm_thro_us,
+                           history[p].largeurPiste90,
+                           //history[p].pwm_thro_us,
                            history[p].pwm_dir_us,
                            history[p].dist,
-                           history[p].states.murs_dlvv,
-                           history[p].states.section,
-                           history[p].states.maxSpeed,
+                           //history[p].states.murs_dlvv,
+                           //history[p].states.section,
+                           //history[p].states.maxSpeed,
                            history[p].states.throttle);
     }
     return;
@@ -297,7 +297,7 @@
     return;
 
 #if DEBUG > 0
-    pc.printf("IT tachy\r\n");
+  //  pc.printf("IT tachy\r\n");
 #endif
 }
 
@@ -361,14 +361,17 @@
     //section de test 1
     p_section1.nextSection =&p_section3;// &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 = 1000.0;//10m
-    p_section1.coef_p = 20.0;
-    p_section1.coef_d = -5.0;
-    p_section1.coef_i = 0.000;
+    p_section1.coef_p90 = 1.0;
+    p_section1.coef_p45 = 1.0;
+    p_section1.coef_d90 = 0.0;
+    p_section1.coef_d45 = 0.05;
+    p_section1.coef_i90 = 0.02;
+    p_section1.coef_i45 = 0.0;
 
     //section de test
     p_section2.nextSection = &p_section3;
@@ -377,10 +380,13 @@
     p_section2.slowSpeed_cmps = 328.0;
     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 = 5.0;
-    p_section2.coef_i = 0.00001;
-    p_section2.coef_d = 0.00001;
+    p_section2.lng_section_cm = 500.0;//2m
+    p_section2.coef_p90 = 10.0;
+    p_section2.coef_d90 = 0.0;
+    p_section2.coef_i90 = 0.0 ;
+    p_section2.coef_p45 = 10.0;
+    p_section2.coef_d45 = 0.0;
+    p_section2.coef_i45 = 0.0 ;
 
     p_section3.nextSection = NULL;
     p_section3.consigne_position = 75.0;
@@ -389,10 +395,12 @@
     p_section3.coef_p_speed = 1.0;
     p_section3.lidarWarningDist_cm = 300.0;
     p_section3.lng_section_cm = 200.0;//2m
-    p_section3.coef_p = 1.0;
-    p_section3.coef_i = 0.2;
-    p_section3.coef_d = 0.0000;
-    
+    p_section3.coef_p90 = 1.0;
+    p_section3.coef_i90 = 0.2;
+    p_section3.coef_d90 = 0.0000;
+    p_section3.coef_p45 = 1.0;
+    p_section3.coef_i45 = 0.2;
+    p_section3.coef_d45 = 0.0000;
     return;
 }
 
@@ -692,11 +700,11 @@
         //application des coefficients
 
         pulseDirection_us = (int)(
-                                ((positionSurPiste45 - 75.0) * p_sectionCourante->coef_p)
-                                //+ ((positionSurPiste90 - 75.0) * p_sectionCourante->coef_p)
-                                + ( -derive45                                  * p_sectionCourante->coef_d)
-                                //+ ( -derive90                                  * p_sectionCourante->coef_d)
-                                //+ ( -integralSum                               * p_sectionCourante->coef_i)
+                                    ((positionSurPiste45 - (largeurPiste45 / 2.0))  * p_sectionCourante->coef_p45)
+                                +   ((positionSurPiste90 - (largeurPiste90 / 2.0))  * p_sectionCourante->coef_p90)
+                                +   ( derive45                                      * p_sectionCourante->coef_d45)
+                                +   ( derive90                                      * p_sectionCourante->coef_d90)
+                                +   ( integralSum                                   * p_sectionCourante->coef_i90)
                             )//(300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul
                             + DIRECTION_PULSE_MIDDLE;