Diff: stateMachines.cpp

Revision:

16:63690703b5b6

Parent:

15:129f205ff030

Child:

17:8c465656eea4

diff -r 129f205ff030 -r 63690703b5b6 stateMachines.cpp
--- a/stateMachines.cpp	Tue Sep 11 12:28:37 2018 +0000
+++ b/stateMachines.cpp	Tue Sep 11 14:49:55 2018 +0000
@@ -31,7 +31,7 @@
 PwmOut      PwmDirection(PB_5); // PWM3 ch2 TIM3
 
 double pulseDirection = 0.0015;
-double pulseSpeed = 0.015;
+double pulseSpeed = INITAL_PULSE_SPEED;
 
 
 //Capteurs direction
@@ -176,13 +176,13 @@
 
     //section de test
     p_section1.nextSection = NULL;
-    p_section1.targetSpeed_cmps = 50;
+    p_section1.targetSpeed_cmps = 100;
     p_section1.slowSpeed_cmps = 15;
     p_section1.brakingCoefficient = 20; // application de la formule
-    p_section1.ms_accel = 0.000004;
-    p_section1.ms_decel = 0.0000001;
+    p_section1.ms_accel = 0.0000003;
+    p_section1.ms_decel = 0.00000015;
     p_section1.lidarWarningDist_cm = 200;
-    p_section1.lng_section_cm = 500;//500cm
+    p_section1.lng_section_cm = 1000;//1000cm
     //p_section1.coef_p = 93400000.0;
     p_section1.coef_p = 35000000.0;
     p_section1.coef_i = 0.0;
@@ -207,7 +207,7 @@
 #ifdef DEBUG
     pc.printf("Init Throttle\r\n");
 #endif
-    st_thro = AT_SPEED;
+    st_thro = UNDER_SPEED;
     PwmMotor.period(0.020);          //20 ms is default
     PwmMotor.pulsewidth(0.0010);
     wait(3);
@@ -215,7 +215,7 @@
     wait(1);
     PwmMotor.pulsewidth(0.0015);
     wait(1);
-    pulseSpeed = 0.0015;
+    pulseSpeed = INITAL_PULSE_SPEED;
 #ifdef DEBUG
     pc.printf("temps init: %d micros\r\n",timeSinceStart.read_us());
     pc.printf("\r\nStates INIT: state Murs: %d, state Section %d, state MaxSpeed %d, state Throttle %d\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
@@ -313,7 +313,7 @@
         }
         break;
     case LOADING_SECTION:
-        if(p_sectionCourante->nextSection != NULL)
+        if(p_sectionCourante != NULL) //la section a ete chargee dans sectionOutput
         {
             st_tmpSection = RUNNING_SECTION;
         }else
@@ -375,10 +375,13 @@
 #endif
     THROTTLE_ST st_tmpThro;
     getTachySpeed();
-
     switch (st_thro) {
     case UNDER_SPEED:
-        if( tachySpeed_cmps > maxSpeed_cmps )
+        if(st_currentSection == ARRET)
+        {
+            st_tmpThro = BRAKING;
+        }
+        else if( tachySpeed_cmps > maxSpeed_cmps )
         {
             st_tmpThro = AT_SPEED;
         }else
@@ -387,7 +390,11 @@
         }
         break;
     case OVER_SPEED:
-        if( tachySpeed_cmps < maxSpeed_cmps )
+        if(st_currentSection == ARRET)
+        {
+            st_tmpThro = BRAKING;
+        }
+        else if( tachySpeed_cmps < maxSpeed_cmps)
         {
             st_tmpThro = AT_SPEED;
         }else
@@ -396,17 +403,39 @@
         }
         break;
     case AT_SPEED:
-        if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS) )
+        if(st_currentSection == ARRET)
+        {
+            st_tmpThro = BRAKING;
+        }
+        else if(tachySpeed_cmps > (maxSpeed_cmps + SPEED_DELTA_CMPS) )
         {
             st_tmpThro = OVER_SPEED;
         } else if( tachySpeed_cmps < (maxSpeed_cmps - SPEED_DELTA_CMPS) )
         {
             st_tmpThro = UNDER_SPEED;
-        }else
+        }
+        else
         {
             return;
         }
         break;
+        case BRAKING:
+        if(st_currentSection == RUNNING_SECTION){
+            if( tachySpeed_cmps < maxSpeed_cmps )
+            {
+                st_tmpThro = UNDER_SPEED;
+            }else if( tachySpeed_cmps > maxSpeed_cmps )
+            {
+                st_tmpThro = OVER_SPEED;
+            }else
+            {
+                st_tmpThro = AT_SPEED;
+            }
+        }
+        else
+        {
+            return;    
+        }
     default:
         break;
     }
@@ -430,12 +459,12 @@
         differenceGD90 = distMurD90Moy - distMurG90Moy;
         differenceGD45 = distMurD45Moy - distMurG45Moy;
         pulseDirection = (double) ((double)((differenceGD45+differenceGD45)/2) / (double)p_sectionCourante->coef_p) + 0.0015;
-        if(pulseDirection > 0.002)
+        if(pulseDirection > MAX_PULSE_WIDTH_FOR_TACHY)
         {
 #if DEBUG > 1
     pc.printf("!!! OVER PWM Direction pulse: %.7f\r\n",pulseDirection);
 #endif
-            pulseDirection = 0.002;
+            pulseDirection = MAX_PULSE_WIDTH_FOR_TACHY;
         }
         else if(pulseDirection < 0.001)
         {
@@ -473,8 +502,7 @@
         tachySectionDist_cm = 0;
         break;
     case ARRET:
-    PwmMotor.pulsewidth(0.00135);
-        wait(10);//on est à l'arret
+        //on est à l'arret
         break;
     default:
         break;
@@ -522,6 +550,13 @@
         }
         break;
     case AT_SPEED:
+    break;
+    case BRAKING:
+#if DEBUG > 2
+        pc.printf("BRAKINGGGGGGGGGGGGGGGGGG !!! \r\n");
+#endif
+        pulseSpeed = BRAKING_PWM;
+        break;
     default:
         break;
     }