Diff: sm_sections.cpp

Revision:

4:efa207509f63

Parent:

3:1b7eb426247e

Child:

6:ab9f3695633f

--- a/sm_sections.cpp	Sun Sep 29 00:43:58 2019 +0000
+++ b/sm_sections.cpp	Sun Sep 29 14:58:03 2019 +0000
@@ -1,36 +1,107 @@
 #include "sm_sections.h"
 
-s_Section* currentSection;
+s_Section* s_currentSection;
 
-s_Section section1;
-s_Section section2;
+s_Section s_section1;
+s_Section s_section2;
 
+E_STATE_SECTIONS e_stateSections;
 
 void init_sm_sections(){
 
+    d_ODOM_distFromSectionStart_m = 0.0;
+    e_stateSections = STOPPED;
+    s_currentSection = NULL;
+
+
  //section de départ
-    section1.nextSection = &section2;
-    section1.consigne_position = 0.75;
-    section1.targetSpeed_mps = 1.0;
-    section1.lng_section_m = 1.0;
-    section1.coef_p = 1.0;
-    section1.coef_d = 0.0;
-    section1.coef_i = 0.000;
+    s_section1.nextSection = &s_section2;
+    s_section1.consigne_position = 0.75;
+    s_section1.targetSpeed_mps = 1.0;
+    s_section1.lng_section_m = 1.0;
+    s_section1.coef_p = 1.0;
+    s_section1.coef_d = 0.0;
+    s_section1.coef_i = 0.000;
 
     //epingle 1
-    section2.nextSection = NULL;
-    section2.consigne_position = 0.75;
-    section2.targetSpeed_mps = 1.0;
-    section2.lng_section_m = 1.0;
-    section2.coef_p = 1.0;
-    section2.coef_d = 0.0;
-    section2.coef_i = 0.000;
+    s_section2.nextSection = NULL;
+    s_section2.consigne_position = 0.75;
+    s_section2.targetSpeed_mps = 1.0;
+    s_section2.lng_section_m = 1.0;
+    s_section2.coef_p = 1.0;
+    s_section2.coef_d = 0.0;
+    s_section2.coef_i = 0.000;
 
     return;
 }
 void update_sm_sections(){
+    
+    E_STATE_SECTIONS e_next_state = e_stateSections;
+
+    pc.printf("\r\nUpdate Section\r\n");
+    
+    if(b_UTILS_flag_button){
+        e_next_state = INIT_SECTION;
+    }
+    else if(s_currentSection == NULL && e_stateSections != INIT_SECTION) {
+        e_next_state = STOPPED;
+    } else {
+
+        switch (e_stateSections) {
+            case INIT_SECTION:
+                e_next_state = LOADING_SECTION;
+            break;
+            case RUNNING_SECTION:
+                if(d_ODOM_distFromSectionStart_m > s_currentSection->lng_section_m) {
+                    e_next_state = LOADING_SECTION;
+                } else if (b_UTILS_flag_emergency_stop){
+                    e_next_state = STOPPED;
+                    }
+                    else{
+                    return;
+                }
+                break;
+            case LOADING_SECTION:
+                if(s_currentSection != NULL && !b_UTILS_flag_emergency_stop) { //la section a ete chargee dans sectionOutput
+                    e_next_state = RUNNING_SECTION;
+                } else {
+                    e_next_state=STOPPED;
+                }
+                break;
+            case STOPPED:
+                if(s_currentSection != NULL) {
+                    e_next_state = RUNNING_SECTION;
+                } else {
+                    return;
+                }
+                break;
+            default:
+                break;
+        }
+    }
+    e_stateSections = e_next_state;
     return;
 }
 void output_sm_sections(){
+
+    pc.printf("\r\n Output Section\r\n");
+    if(s_currentSection !=NULL) {
+        switch (e_stateSections) {
+            case INIT_SECTION:
+            b_UTILS_flag_button = false;
+            s_currentSection = &s_section1;
+            case RUNNING_SECTION:
+                break;
+            case LOADING_SECTION:
+                s_currentSection=s_currentSection->nextSection;
+                d_ODOM_distFromSectionStart_m = 0.0;
+                break;
+            case STOPPED:
+                //on est à l'arret
+                break;
+            default:
+                break;
+        }
+    }
     return;
 }