Diff: stateMachines.cpp

Revision:

15:129f205ff030

Parent:

14:d471faa7d1a2

Child:

16:63690703b5b6

diff -r d471faa7d1a2 -r 129f205ff030 stateMachines.cpp
--- a/stateMachines.cpp	Tue Sep 11 09:55:49 2018 +0000
+++ b/stateMachines.cpp	Tue Sep 11 12:28:37 2018 +0000
@@ -19,9 +19,6 @@
 uint16_t distMurG10Moy;
 uint16_t distMurD10Moy;
 uint16_t distMurFrontMoy
-
-
-
 #endif
 int dist_obstacle_lidar[NB_ECHANTILLONS_LIDAR];//buffer tournant
 int index_fifo_ir = 0;//pour géreer le buffer tournant
@@ -29,12 +26,12 @@
 //sections
 s_Section p_section1;
 
-//Controls
+//PWM Controls
 PwmOut      PwmMotor(PB_6);     // PWM4 ch1 TIM4
 PwmOut      PwmDirection(PB_5); // PWM3 ch2 TIM3
 
-float pulseDirection = 0.0015;
-float pulseSpeed = 0.015;
+double pulseDirection = 0.0015;
+double pulseSpeed = 0.015;
 
 
 //Capteurs direction
@@ -70,7 +67,6 @@
 int tachySpeed_cmps = 0;
 int tachyStepsRegister = 0;
 int tachySectionDist_cm = 0;
-float ms_pwmSpeedPulse = 0.0015;//IDLE
 
 //Etats
 MUR_ST st_murs;
@@ -96,11 +92,11 @@
     {
         return;//on attend encore un peu l'aquisition de la vitesse
     }
-    tachySectionDist_cm += tachyStepsRegister * 4;
-#ifdef DEBUG
-    pc.printf("IT: distance parcourue %d\r\n",tachySectionDist_cm);
-#endif
-    tachySpeed_cmps = (tachyStepsRegister * 40000)/timerSinceGetTachy.read_us();
+    tachySectionDist_cm += tachyStepsRegister;
+    tachySpeed_cmps = (tachyStepsRegister * 1000000)/timerSinceGetTachy.read_us();
+    #if DEBUG > 2
+        pc.printf("IT: distance parcourue %d       , vitesse:%d         \r\n",tachySectionDist_cm,tachySpeed_cmps);
+    #endif
     tachyStepsRegister=0;
     timerSinceGetTachy.reset();
     timerSinceGetTachy.start();
@@ -183,12 +179,12 @@
     p_section1.targetSpeed_cmps = 50;
     p_section1.slowSpeed_cmps = 15;
     p_section1.brakingCoefficient = 20; // application de la formule
-    p_section1.ms_accel = 0.00005;
-    p_section1.ms_decel = 0.0001;
+    p_section1.ms_accel = 0.000004;
+    p_section1.ms_decel = 0.0000001;
     p_section1.lidarWarningDist_cm = 200;
-    p_section1.lng_section_cm = 1000;//10m
+    p_section1.lng_section_cm = 500;//500cm
     //p_section1.coef_p = 93400000.0;
-    p_section1.coef_p = 25000000.0;
+    p_section1.coef_p = 35000000.0;
     p_section1.coef_i = 0.0;
     p_section1.coef_d = 0.0;
     return;
@@ -219,6 +215,7 @@
     wait(1);
     PwmMotor.pulsewidth(0.0015);
     wait(1);
+    pulseSpeed = 0.0015;
 #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);
@@ -433,11 +430,25 @@
         differenceGD90 = distMurD90Moy - distMurG90Moy;
         differenceGD45 = distMurD45Moy - distMurG45Moy;
         pulseDirection = (double) ((double)((differenceGD45+differenceGD45)/2) / (double)p_sectionCourante->coef_p) + 0.0015;
+        if(pulseDirection > 0.002)
+        {
+#if DEBUG > 1
+    pc.printf("!!! OVER PWM Direction pulse: %.7f\r\n",pulseDirection);
+#endif
+            pulseDirection = 0.002;
+        }
+        else if(pulseDirection < 0.001)
+        {
+#if DEBUG > 1
+    pc.printf("!!! UNDER PWM Direction pulse: %.7f\r\n",pulseDirection);
+#endif
+            pulseDirection = 0.001;
+        }
     default:
         break;
     }
     #if DEBUG > 1
-    pc.printf("PWM pulse: %.7f\r\n",pulseDirection);
+    pc.printf("PWM Direction pulse: %.7f\r\n",pulseDirection);
     #endif
     PwmDirection.pulsewidth(pulseDirection);
     return;
@@ -462,6 +473,7 @@
         tachySectionDist_cm = 0;
         break;
     case ARRET:
+    PwmMotor.pulsewidth(0.00135);
         wait(10);//on est à l'arret
         break;
     default:
@@ -498,19 +510,29 @@
 #endif
     switch (st_thro) {
     case UNDER_SPEED:
-        ms_pwmSpeedPulse += p_sectionCourante->ms_accel;
+        if ( (pulseSpeed + p_sectionCourante->ms_decel) < 0.002)
+        {
+            pulseSpeed += p_sectionCourante->ms_accel;
+        }
         break;
     case OVER_SPEED:
-        ms_pwmSpeedPulse -= p_sectionCourante->ms_decel;
+        if ( (pulseSpeed - p_sectionCourante->ms_decel) > 0.0015)
+        {
+            pulseSpeed -= p_sectionCourante->ms_decel;
+        }
         break;
     case AT_SPEED:
     default:
         break;
     }
+    PwmMotor.pulsewidth(pulseSpeed);
+#if DEBUG > 1
+    pc.printf("PWM Thro pulse: %.7f\r\n",pulseSpeed);
+#endif
 #if DEBUG > 0
     pc.printf("\r\nodo:%d  dist = %d    \tstrength = %d    \tC45D: %d C45G: %d  C90D: %d  C90G: %d  looptime: %d micros",tachySectionDist_cm,distLidar,strengthLidar,distMurD45Moy,distMurG45Moy,distMurD90Moy,distMurG90Moy,timeSinceStart.read_us());// output signal strength value
     pc.printf("\r\nstate Murs: %d, state Section %d, state MaxSpeed %d, state Throttle %d\r\n",st_murs,st_currentSection,st_maxSpeed,st_thro);
-    wait(2);
+    //wait(2);
     timeSinceStart.reset();
     timeSinceStart.start();
 #endif