Diff: sm_sections.cpp

Revision:

8:f23601373e8b

Parent:

6:ab9f3695633f

Child:

9:1b54bac6d9a7

--- a/sm_sections.cpp	Sun Sep 29 20:22:43 2019 +0000
+++ b/sm_sections.cpp	Wed Oct 02 22:25:12 2019 +0000
@@ -1,23 +1,26 @@
 #include "sm_sections.h"
+#include "sm_mpu.h"
 
 
 s_Section s_section1;
 s_Section s_section2;
 
-E_STATE_SECTIONS e_stateSections;
+E_STATE_SECTIONS e_SECTIONS_state;
+
 
-void init_sm_sections(){
+void init_sm_sections()
+{
 
     d_ODOM_distFromSectionStart_m = 0.0;
-    e_stateSections = STOPPED;
-    s_UTILS_currentSection = NULL;
+    e_SECTIONS_state = STOPPED;
+    s_section1.nextSection = NULL;
 
 
- //section de départ
+//section de départ
     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.targetSpeed_mps = 1640;
+    s_section1.lng_section_m = 4.0;
     s_section1.coef_p = 1.0;
     s_section1.coef_d = 0.0;
     s_section1.coef_i = 0.000;
@@ -25,82 +28,78 @@
     //epingle 1
     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.targetSpeed_mps = 1640;
+    s_section2.lng_section_m = 4.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;
+void update_sm_sections()
+{
+
+    E_STATE_SECTIONS e_next_state = e_SECTIONS_state;
 
-    //pc.printf("\r\nUpdate Section\r\n");
-    
-    if(b_UTILS_flag_button){
-        e_next_state = INIT_SECTION;
-    }
-    else if(s_UTILS_currentSection == NULL && e_stateSections != INIT_SECTION) {
+    //rs_UTILS_pc.printf("\r\nUpdate Section\r\n");
+
+    if(b_UTILS_flag_emergency_stop) {
+        rs_UTILS_pc.printf("emergency stop -> STOPPED\r\n");
         e_next_state = STOPPED;
     } else {
-
-        switch (e_stateSections) {
+        switch (e_SECTIONS_state) {
             case INIT_SECTION:
-                e_next_state = LOADING_SECTION;
-            break;
+                e_next_state = RUNNING_SECTION;
+                //rs_UTILS_pc.printf("RUNNING SECTION\r\n");
+                break;
             case RUNNING_SECTION:
                 if(d_ODOM_distFromSectionStart_m > s_UTILS_currentSection->lng_section_m) {
                     e_next_state = LOADING_SECTION;
-                } else if (b_UTILS_flag_emergency_stop){
-                    e_next_state = STOPPED;
-                    }
-                    else{
-                    return;
+                    //rs_UTILS_pc.printf("LOADING SECTION !\r\n");
                 }
                 break;
             case LOADING_SECTION:
-                if(s_UTILS_currentSection != NULL && !b_UTILS_flag_emergency_stop) { //la section a ete chargee dans sectionOutput
+                if(s_UTILS_currentSection != NULL) {
                     e_next_state = RUNNING_SECTION;
+                    //rs_UTILS_pc.printf("RUNNING SECTION !\r\n");
                 } else {
-                    e_next_state=STOPPED;
+                    b_UTILS_flag_emergency_stop = true;
+                    e_next_state = STOPPED;
                 }
                 break;
             case STOPPED:
-                if(s_UTILS_currentSection != NULL) {
-                    e_next_state = RUNNING_SECTION;
-                } else {
-                    return;
+                if(b_UTILS_flag_button_SECTIONS) {
+                    e_next_state = INIT_SECTION;
+                    rs_UTILS_pc.printf("INIT SECTION !\r\n");
                 }
                 break;
             default:
                 break;
         }
     }
-    e_stateSections = e_next_state;
+    e_SECTIONS_state = e_next_state;
     return;
 }
-void output_sm_sections(){
-
-    //pc.printf("\r\n Output Section\r\n");
-    if(s_UTILS_currentSection !=NULL) {
-        switch (e_stateSections) {
-            case INIT_SECTION:
-            b_UTILS_flag_button = false;
+void output_sm_sections()
+{
+    switch (e_SECTIONS_state) {
+        case INIT_SECTION:
+            b_UTILS_flag_button_SECTIONS = false;
             s_UTILS_currentSection = &s_section1;
-            case RUNNING_SECTION:
-                break;
-            case LOADING_SECTION:
-                s_UTILS_currentSection=s_UTILS_currentSection->nextSection;
-                d_ODOM_distFromSectionStart_m = 0.0;
-                break;
-            case STOPPED:
-                //on est à l'arret
-                break;
-            default:
-                break;
-        }
+            rs_UTILS_pc.printf("sect 1 INITIALIZED\r\n");
+             mpu_log_start(10000);
+        case RUNNING_SECTION:
+            break;
+        case LOADING_SECTION:
+            //rs_UTILS_pc.printf("next section loading\r\n");
+            s_UTILS_currentSection=s_UTILS_currentSection->nextSection;
+            d_ODOM_distFromSectionStart_m = 0.0;
+            break;
+        case STOPPED:
+            //on est à l'arret
+            break;
+        default:
+            break;
     }
     return;
 }