Revision 52:228703200e35, committed 2018-09-21

Comitter:

ohlimi2

Date:

Fri Sep 21 17:29:30 2018 +0000

Parent:

51:09ecba68d0cf

Commit message:

version bof

Changed in this revision

--- a/stateMachines.cpp	Fri Sep 21 13:17:24 2018 +0000
+++ b/stateMachines.cpp	Fri Sep 21 17:29:30 2018 +0000
@@ -86,6 +86,8 @@
 //LIDAR
 Serial serialLidar(PC_10,PC_11);  // tx, rx
 
+Serial serialDebug(PC_12,PD_2);  // tx, rx
+
 int distLidar;// LiDAR actually measured distance value
 int distLidarPrev;
 int strengthLidar;// LiDAR signal strength
@@ -360,34 +362,49 @@
     tachyStepsRegister = 0;
 
     //section de test 1
-    p_section1.nextSection =&p_section3;// &p_section2;
+    p_section1.nextSection =&p_section2;// &p_section2;
     p_section1.consigne_position = 75.0;
-    p_section1.targetSpeed_cmps = 400.0;
+    p_section1.targetSpeed_cmps = 600.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_p90 = 4.0;
-    p_section1.coef_d90 = 0.0;
+    p_section1.lng_section_cm = 1300.0;//13m
+    // coef P
+    p_section1.coef_pd90 = -0.03;
+    p_section1.coef_pg90 =  0.03;
+    p_section1.coef_pd45 = -0.03;
+    p_section1.coef_pg45 =  0.03;
+    // le reste
+    p_section1.coef_dd90 = 0.0;
+    p_section1.coef_dg90 = 0.0;
+    p_section1.coef_dd45 = 0.0;
+    p_section1.coef_dg45 = 0.0;
+        
     p_section1.coef_i90 = 0.0;
-    p_section1.coef_p45 = 0.0;
-    p_section1.coef_d45 = 0.0;
     p_section1.coef_i45 = 0.0;
 
     //section de test
     p_section2.nextSection = &p_section3;
     p_section2.consigne_position = 75.0;
-    p_section2.targetSpeed_cmps = 328.0;
+    p_section2.targetSpeed_cmps = 400.0;
     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_p90 = 1.0;
-    p_section2.coef_d90 = 0.0;
+    p_section2.lng_section_cm = 600.0;//2m
+    // coef en P
+    p_section2.coef_pd90 = -0.03;
+    p_section2.coef_pg90 =  0.03;
+    p_section2.coef_pd45 = -0.03;
+    p_section2.coef_pg45 =  0.03;
+    // le reste
+    p_section2.coef_dd90 = 0.0;
+    p_section2.coef_dg90 = 0.0;
+    p_section2.coef_dd45 = 0.0;
+    p_section2.coef_dg45 = 0.0;
+    
     p_section2.coef_i90 = 0.0;
-    p_section2.coef_p45 = 1.0;
-    p_section2.coef_d45 = 0.0;
     p_section2.coef_i45 = 0.0;
+    
 
     p_section3.nextSection = NULL;
     p_section3.consigne_position = 75.0;
@@ -395,13 +412,20 @@
     p_section3.slowSpeed_cmps = 0.0;
     p_section3.coef_p_speed = 1.0;
     p_section3.lidarWarningDist_cm = 300.0;
-    p_section3.lng_section_cm = 200.0;//2m
-    p_section3.coef_p90 = 1.0;
-    p_section3.coef_d90 = 0.0;
+    p_section3.lng_section_cm = 100.0;//2m
+    // coef en P
+    p_section3.coef_pd90 = -0.03;
+    p_section3.coef_pg90 =  0.03;
+    p_section3.coef_pd45 = -0.03;
+    p_section3.coef_pg45 =  0.03;
+    
+    p_section3.coef_dd90 = 0.0;
+    p_section3.coef_dg90 = 0.0;
+    p_section3.coef_dd45 = 0.0;
+    p_section3.coef_dg45 = 0.0;
+    
+    p_section3.coef_i45 = 0.0;
     p_section3.coef_i90 = 0.0;
-    p_section3.coef_p45 = 1.0;
-    p_section3.coef_d45 = 0.0;
-    p_section3.coef_i45 = 0.0;
     
     return;
 }
@@ -414,6 +438,7 @@
     st_maxSpeed=SPEED_MAX;
     maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps;
     serialLidar.baud(115200);
+    serialDebug.baud(115200);
     serialLidar.attach(&it_serial);
     return;
 }
@@ -458,14 +483,14 @@
     distMurD45Moy = getDistMoy(&anaD45,distMurD45,NB_ECHANTILLONS_IR);
     distMurG90Moy = getDistMoy(&anaG90,distMurG90,NB_ECHANTILLONS_IR);
     distMurD90Moy = getDistMoy(&anaD90,distMurD90,NB_ECHANTILLONS_IR);
-    shortDistMurG90Moy = getShortDistMoy(&anaShortG90,shortDistMurD90,NB_ECHANTILLONS_IR);
-    shortDistMurD90Moy = getShortDistMoy(&anaShortD90,shortDistMurD90,NB_ECHANTILLONS_IR);
+    //shortDistMurG90Moy = getShortDistMoy(&anaShortG90,shortDistMurD90,NB_ECHANTILLONS_IR);
+    //shortDistMurD90Moy = getShortDistMoy(&anaShortD90,shortDistMurD90,NB_ECHANTILLONS_IR);
     index_fifo_ir = (index_fifo_ir+1)%NB_ECHANTILLONS_IR;
 
 
     trueDistMurD90Moy = distMurD90Moy;
     trueDistMurG90Moy = distMurG90Moy;
-
+/*
     if(shortDistMurG90Moy < DIST_MIN_LONG_CM) {
         trueDistMurG90Moy = shortDistMurG90Moy;
     } else {
@@ -476,7 +501,7 @@
     } else {
         trueDistMurD90Moy = distMurD90Moy;
     }
-
+*/
 
 #ifdef DLVV
     switch (st_obstacle) {
@@ -639,6 +664,17 @@
         //pour dériver
         positionSurPiste45Prev = positionSurPiste45;
         positionSurPiste90Prev = positionSurPiste90;
+        
+    //    distMurG45MoyPrev = distMurG45Moy ;
+    //    distMurD45MoyPrev = distMurD45Moy ;
+     //   distMurD45MoyPrev = distMurD45Moy ;
+     //   distMurD45MoyPrev = distMurD45Moy ;
+        
+        
+        
+        
+        
+        
 #if (DEBUG > 3)
         pc.printf("\r\n Output MURS\r\n");
 #endif
@@ -676,17 +712,8 @@
         derive45 = positionSurPiste45 - positionSurPiste45Prev;
         derive90 = positionSurPiste90 - positionSurPiste90Prev;
 
-#ifdef OMAR/*
-        pc.printf("derive45 => %.4lf \r\n  ",derive45);
-        pc.printf("derive90 => %.4lf \r\n  ",derive90);
-        pc.printf("distMurG45Moy => %.4lf \r\n  ",distMurG45Moy);
-        pc.printf("distMurD45Moy => %.4lf \r\n  ",distMurD45Moy);
-        pc.printf("trueDistMurG90Moy => %.4lf \r\n  ",trueDistMurG90Moy);
-        pc.printf("trueDistMurD90Moy => %.4lf \r\n  ",trueDistMurD90Moy);
-        pc.printf("positionSurPiste90 => %.4lf \r\n  ",positionSurPiste90);
-        pc.printf("positionSurPiste45 => %.4lf \r\n  ",positionSurPiste45);
-        pc.printf("largeurPiste90 => %.4lf \r\n  ",largeurPiste90);*/
-#endif
+
+
 
         //integral correction
         lastDifferences90[lastDifferenceIndex] = (p_sectionCourante->consigne_position - positionSurPiste90);
@@ -702,13 +729,34 @@
         //application des coefficients
 
         pulseDirection_us = (int)(
-                                ((positionSurPiste45 - (largeurPiste45/2)) * p_sectionCourante->coef_p45)
-                                + ((positionSurPiste90 - (largeurPiste90/2)) * p_sectionCourante->coef_p90)
-                                + ( -derive45                                  * p_sectionCourante->coef_d45)
-                                + ( -derive90                                  * p_sectionCourante->coef_d90)
-                                + ( -integralSum                               * p_sectionCourante->coef_i90)
+                                  ((distMurG45Moy / 0.27) * p_sectionCourante->coef_pg45)
+                                + ((distMurD45Moy / 0.27) * p_sectionCourante->coef_pd45)
+                                + ((distMurD90Moy / 0.17) * p_sectionCourante->coef_pg90)
+                                + ((distMurD90Moy / 0.17) * p_sectionCourante->coef_pd90)
+            
+                                
+                                //+ ( -integralSum                               * p_sectionCourante->coef_i90)
                             )//(300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul
                             + DIRECTION_PULSE_MIDDLE;
+    
+
+
+
+
+
+
+
+
+/*
+        serialDebug.printf("derive45 => %.4lf \r\n  ",derive45);
+        serialDebug.printf("derive90 => %.4lf \r\n  ",derive90);
+        serialDebug.printf("distMurG45Moy => %.4lf \r\n  ",distMurG45Moy);
+        serialDebug.printf("distMurD45Moy => %.4lf \r\n  ",distMurD45Moy);
+        serialDebug.printf("trueDistMurG90Moy => %.4lf \r\n  ",trueDistMurG90Moy);
+        serialDebug.printf("trueDistMurD90Moy => %.4lf \r\n  ",trueDistMurD90Moy);
+        serialDebug.printf("positionSurPiste90 => %.4lf \r\n  ",positionSurPiste90);
+        serialDebug.printf("positionSurPiste45 => %.4lf \r\n  ",positionSurPiste45);
+        serialDebug.printf("largeurPiste90 => %.4lf \r\n  ",largeurPiste90);*/
 
 
 #ifdef OMAR

--- a/stateMachines.h	Fri Sep 21 13:17:24 2018 +0000
+++ b/stateMachines.h	Fri Sep 21 17:29:30 2018 +0000
@@ -117,12 +117,19 @@
    double consigne_position;
    double lidarWarningDist_cm;
    double lng_section_cm;
-   double coef_p45;
+   // coef
+   double coef_pd45;
+   double coef_pg45;
+   double coef_pd90;
+   double coef_pg90;
+    //
+    double coef_dd45;
+    double coef_dg45;
+    double coef_dd90;
+    double coef_dg90;
+    
    double coef_i45;
-   double coef_d45;
-   double coef_p90;
    double coef_i90;
-   double coef_d90;
    s_section* nextSection;
 }s_Section;