Diff: stateMachines.cpp

Revision:

14:d471faa7d1a2

Parent:

13:af9a59ccf60b

Child:

15:129f205ff030

--- a/stateMachines.cpp	Mon Sep 10 21:00:47 2018 +0000
+++ b/stateMachines.cpp	Tue Sep 11 09:55:49 2018 +0000
@@ -49,7 +49,7 @@
 AnalogIn anaDlvvFront(PA_4);//capteur ir avant
 #endif
 
-int differenceGD45;
+int differenceGD45,differenceGD90;
 
 //Capteur vitesse
 InterruptIn it_tachymeter(PA_11);
@@ -97,6 +97,9 @@
         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();
     tachyStepsRegister=0;
     timerSinceGetTachy.reset();
@@ -111,16 +114,15 @@
     {
         sumMoy+=tab[k];
     }
-#if (NB_ECHANTILLONS_IR == 4 && NB_ECHANTILLONS_LIDAR == 4)
-    return (uint16_t)sumMoy >> 2;
-#else
     return sumMoy/size;
-#endif
 }
 
 void it4cm()
 {
     tachyStepsRegister+=4;
+#ifdef DEBUG
+    pc.printf("IT tachy\r\n");
+#endif
 }
 
 void it_serial()
@@ -169,7 +171,7 @@
     pc.printf("Init Section\r\n");
 #endif
     st_currentSection=ARRET;
-    p_sectionCourante->nextSection=&p_section1;
+    p_sectionCourante=&p_section1;
     it_tachymeter.fall(&it4cm);
     timerSinceGetTachy.start();
     getTachySpeed();//to reset
@@ -185,7 +187,8 @@
     p_section1.ms_decel = 0.0001;
     p_section1.lidarWarningDist_cm = 200;
     p_section1.lng_section_cm = 1000;//10m
-    p_section1.coef_p = 93400000.0;
+    //p_section1.coef_p = 93400000.0;
+    p_section1.coef_p = 25000000.0;
     p_section1.coef_i = 0.0;
     p_section1.coef_d = 0.0;
     return;
@@ -199,6 +202,7 @@
     st_maxSpeed=SPEED_MAX;
     maxSpeed_cmps= p_sectionCourante->targetSpeed_cmps;
     serialLidar.baud(115200);
+    serialLidar.attach(&it_serial);
     return;
 }
 
@@ -228,6 +232,9 @@
 //########## UPDATE STATES ##########
 void mursUpdate(void)
 {
+#if (DEBUG > 3)
+    pc.printf("\r\nUpdate MURS\r\n");
+#endif
     MUR_ST st_tmpMurs;
     //lectures
     distMurG90[index_fifo_ir]=anaG90.read_u16();
@@ -238,6 +245,10 @@
 
     distMurG45Moy = getDistMoy(distMurG45,NB_ECHANTILLONS_IR);
     distMurD45Moy = getDistMoy(distMurD45,NB_ECHANTILLONS_IR);
+    
+#if DEBUG > 1
+    pc.printf("dist45G:%d       dist45D:%d        Deadzone: %d\r\n",distMurG45Moy,distMurD45Moy,IR_DEADZONE_U16_22cm);
+#endif
 
     switch (st_murs) {
     case EQUILIBRAGE_REPULSIF:
@@ -289,6 +300,9 @@
 
 void sectionUpdate(void)
 {
+#if (DEBUG > 3)
+    pc.printf("\r\nUpdate Section\r\n");
+#endif
     SECTION_ST st_tmpSection;
     switch (st_currentSection) {
     case RUNNING_SECTION:
@@ -296,6 +310,10 @@
         {
             st_tmpSection = LOADING_SECTION;
         }
+        else
+        {
+            return;
+        }
         break;
     case LOADING_SECTION:
         if(p_sectionCourante->nextSection != NULL)
@@ -307,7 +325,7 @@
         }
         break;
     case ARRET:
-        if(p_sectionCourante->nextSection != NULL)
+        if(p_sectionCourante != NULL)
         {
             st_tmpSection = RUNNING_SECTION;
         }
@@ -325,6 +343,9 @@
 
 void maxSpeedUpdate(void)
 {
+#if (DEBUG > 3)
+    pc.printf("\r\nUpdate MaxSpeed\r\n");
+#endif
     MAX_SPEED_ST st_tmpMaxSpeed;
     i=0;
 
@@ -352,7 +373,11 @@
 
 void throttleUpdate(void)
 {
+#if (DEBUG > 3)
+    pc.printf("\r\nUpdate Throttle\r\n");
+#endif
     THROTTLE_ST st_tmpThro;
+    getTachySpeed();
 
     switch (st_thro) {
     case UNDER_SPEED:
@@ -397,15 +422,23 @@
 //updating output parameters
 void mursOutput(void)
 {
+#if (DEBUG > 3)
+    pc.printf("\r\n Output MURS\r\n");
+#endif
     switch (st_murs) {
     case EQUILIBRAGE_REPULSIF://TO DO TODO
     case ATTRACTIF_G:
     case ATTRACTIF_D:
+    
+        differenceGD90 = distMurD90Moy - distMurG90Moy;
         differenceGD45 = distMurD45Moy - distMurG45Moy;
-        pulseDirection = (double) ((double)differenceGD45 / (double)p_sectionCourante->coef_p) + 0.0015;
+        pulseDirection = (double) ((double)((differenceGD45+differenceGD45)/2) / (double)p_sectionCourante->coef_p) + 0.0015;
     default:
         break;
     }
+    #if DEBUG > 1
+    pc.printf("PWM pulse: %.7f\r\n",pulseDirection);
+    #endif
     PwmDirection.pulsewidth(pulseDirection);
     return;
 }
@@ -418,7 +451,9 @@
 #endif
 void sectionOutput(void)
 {
-
+#if (DEBUG > 3)
+    pc.printf("\r\n Output Section\r\n");
+#endif
     switch (st_currentSection) {
     case RUNNING_SECTION:
         break;
@@ -438,6 +473,9 @@
 
 void maxSpeedOutput(void)
 {
+#if (DEBUG > 3)
+    pc.printf("\r\n Output MAX SPEED\r\n");
+#endif
     switch(st_maxSpeed)
     {
     case SPEED_LIMITED:
@@ -455,6 +493,9 @@
 
 void throttleOutput(void)
 {
+#if (DEBUG > 3)
+    pc.printf("\r\n Output TROTTLE\r\n");
+#endif
     switch (st_thro) {
     case UNDER_SPEED:
         ms_pwmSpeedPulse += p_sectionCourante->ms_accel;
@@ -466,9 +507,10 @@
     default:
         break;
     }
-#ifdef DEBUG
+#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);
     timeSinceStart.reset();
     timeSinceStart.start();
 #endif