Diff: sm_esc.cpp

Revision:

4:efa207509f63

Parent:

3:1b7eb426247e

Child:

5:8bbe640528bc

--- a/sm_esc.cpp	Sun Sep 29 00:43:58 2019 +0000
+++ b/sm_esc.cpp	Sun Sep 29 14:58:03 2019 +0000
@@ -18,7 +18,6 @@
         case ESC_DISABLED:
             if(b_UTILS_flag_button && !b_UTILS_flag_emergency_stop) {
                 next_state = ESC_INIT;
-                b_UTILS_flag_button = false;
             }
             break;
         case ESC_INIT:
@@ -43,13 +42,17 @@
      switch(e_stateESC) {
         case ESC_DISABLED:
             if(b_UTILS_flag_emergency_stop){
+                pc.printf("EMERGENCY STOP\r\n");
+                b_UTILS_flag_emergency_stop = false;
                 relay_ESC = 1;
                 wait(0.5);
                 relay_ESC = 0;
             }
             break;
+            
         case ESC_INIT:
-            pc.printf("init the ESC\n");
+            pc.printf("Init esc\r\n");
+            b_UTILS_flag_button = false;
             pwm_ESC.period_ms(ESC_PERIOD_DURATION_MS);          //20 ms is default
             pwm_ESC.pulsewidth_us(0);
             
@@ -65,31 +68,15 @@
             pwm_ESC.pulsewidth_us(ESC_PULSE_IDLE_US);
             wait(1);
             break;
+            
         case ESC_COMMAND:
             wait(1);
-            pc.printf("send pulse 1600 to esc\n");
-            pwm_ESC.pulsewidth_us(1600);
-            wait(1);
-            pc.printf("send pulse 1700 to esc\n");
-            pwm_ESC.pulsewidth_us(1700);
-            wait(1);
-            
-            pc.printf("send pulse 1300 to esc\n");
-            pwm_ESC.pulsewidth_us(1300);
-            wait(3);
+
+            pc.printf("send pulse 1620 to esc\r\n");
+            pwm_ESC.pulsewidth_us(1620);
+            wait(10);
             
-            pc.printf("send pulse 1600 to esc\n");
-            pwm_ESC.pulsewidth_us(1600);
-            wait(1);
-            pc.printf("send pulse 1700 to esc\n");
-            pwm_ESC.pulsewidth_us(1700);
-            wait(1);
-            
-            pc.printf("send pulse 1500 to esc\n");
-            pwm_ESC.pulsewidth_us(1500);
-            wait(1);
-            
-            pc.printf("emergency stop\n");
+            pc.printf("emergency stop flag raised\r\n");
             b_UTILS_flag_emergency_stop = true;
             break;
         default: