Diff: stateMachines.cpp

Revision:

46:9262b07b0833

Parent:

45:7d67809bd7bf

Child:

47:b17061738568

--- a/stateMachines.cpp	Thu Sep 20 12:11:11 2018 +0000
+++ b/stateMachines.cpp	Thu Sep 20 18:45:07 2018 +0000
@@ -44,6 +44,7 @@
 //sections
 s_Section p_section1;
 s_Section p_section2;
+s_Section p_section3;
 
 //PWM Controls
 PwmOut      PwmMotor(PB_6);     // PWM4 ch1 TIM4
@@ -357,20 +358,19 @@
     tachyStepsRegister = 0;
 
     //section de test 1
-    p_section1.nextSection =NULL;// &p_section2;
+    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 = 2000.0;//20m
-    p_section1.coef_p = 0.40;
-    p_section1.coef_d = 0.05;
-    p_section1.coef_i = 0.001;
+    p_section1.lng_section_cm = 1000.0;//10m
+    p_section1.coef_p = 4.0;
+    p_section1.coef_d = 0.2;
+    p_section1.coef_i = 0.000;
 
     //section de test
-    p_section2.nextSection = NULL;
+    p_section2.nextSection = &p_section3;
     p_section2.consigne_position = 75.0;
     p_section2.targetSpeed_cmps = 328.0;
     p_section2.slowSpeed_cmps = 328.0;
@@ -381,6 +381,17 @@
     p_section2.coef_i = 0.00001;
     p_section2.coef_d = 0.00001;
 
+    p_section3.nextSection = NULL;
+    p_section3.consigne_position = 75.0;
+    p_section3.targetSpeed_cmps = 0.0;
+    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_p = 1.0;
+    p_section3.coef_i = 0.2;
+    p_section3.coef_d = 0.0000;
+    
     return;
 }
 
@@ -680,12 +691,12 @@
         //application des coefficients
 
         pulseDirection_us = (int)(
-                                ((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
+                                ((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)
+                            )* 1;//(300.0/(tachySpeed_cmps+1.0)) // faire du test reel pour afiné ou un calcul
                             + DIRECTION_PULSE_MIDDLE;