Diff: sm_esc.cpp

Revision:

3:1b7eb426247e

Parent:

2:e9d928fd327a

Child:

4:efa207509f63

--- a/sm_esc.cpp	Sat Sep 28 22:58:02 2019 +0000
+++ b/sm_esc.cpp	Sun Sep 29 00:43:58 2019 +0000
@@ -12,11 +12,11 @@
 
 void update_sm_esc()
 {
-    E_STATE_ESC next_state;
+    E_STATE_ESC next_state = e_stateESC;
 
     switch(e_stateESC) {
         case ESC_DISABLED:
-            if(b_UTILS_flag_button) {
+            if(b_UTILS_flag_button && !b_UTILS_flag_emergency_stop) {
                 next_state = ESC_INIT;
                 b_UTILS_flag_button = false;
             }
@@ -42,36 +42,54 @@
 {
      switch(e_stateESC) {
         case ESC_DISABLED:
-            relay_ESC = 0;
+            if(b_UTILS_flag_emergency_stop){
+                relay_ESC = 1;
+                wait(0.5);
+                relay_ESC = 0;
+            }
             break;
         case ESC_INIT:
-            pc.printf("init the ESC");
+            pc.printf("init the ESC\n");
+            pwm_ESC.period_ms(ESC_PERIOD_DURATION_MS);          //20 ms is default
+            pwm_ESC.pulsewidth_us(0);
+            
             relay_ESC = 1;
-            wait(0.2);
-            pwm_ESC.period_ms(ESC_PERIOD_DURATION_MS);          //20 ms is default
-            pwm_ESC.pulsewidth_us(600);
+            wait(0.5);
+            relay_ESC = 0;
+            wait(0.1);
+            
+            pwm_ESC.pulsewidth_us(ESC_PULSE_MIN_US);
+            wait(3);
+            pwm_ESC.pulsewidth_us(ESC_PULSE_MAX_US);
+            wait(1);
+            pwm_ESC.pulsewidth_us(ESC_PULSE_IDLE_US);
             wait(1);
             break;
         case ESC_COMMAND:
-            pc.printf("send pulse 1500 to esc");
-            pwm_ESC.pulsewidth_us(1500);
             wait(1);
-            pc.printf("send pulse 1600 to esc");
+            pc.printf("send pulse 1600 to esc\n");
             pwm_ESC.pulsewidth_us(1600);
             wait(1);
-            pc.printf("send pulse 1700 to esc");
+            pc.printf("send pulse 1700 to esc\n");
             pwm_ESC.pulsewidth_us(1700);
             wait(1);
-            pc.printf("send pulse 1800 to esc");
-            pwm_ESC.pulsewidth_us(1800);
+            
+            pc.printf("send pulse 1300 to esc\n");
+            pwm_ESC.pulsewidth_us(1300);
+            wait(3);
+            
+            pc.printf("send pulse 1600 to esc\n");
+            pwm_ESC.pulsewidth_us(1600);
             wait(1);
-            pc.printf("send pulse 1900 to esc");
-            pwm_ESC.pulsewidth_us(1900);
+            pc.printf("send pulse 1700 to esc\n");
+            pwm_ESC.pulsewidth_us(1700);
             wait(1);
-            pc.printf("send pulse 2000 to esc");
-            pwm_ESC.pulsewidth_us(2000);
+            
+            pc.printf("send pulse 1500 to esc\n");
+            pwm_ESC.pulsewidth_us(1500);
             wait(1);
-            pc.printf("emergency stop");
+            
+            pc.printf("emergency stop\n");
             b_UTILS_flag_emergency_stop = true;
             break;
         default: