Diff: sm_esc.cpp

Revision:

5:8bbe640528bc

Parent:

4:efa207509f63

Child:

6:ab9f3695633f

--- a/sm_esc.cpp	Sun Sep 29 14:58:03 2019 +0000
+++ b/sm_esc.cpp	Sun Sep 29 17:09:48 2019 +0000
@@ -3,10 +3,18 @@
 PwmOut pwm_ESC(PD_12);
 DigitalOut relay_ESC(PG_0);
 E_STATE_ESC e_stateESC;
+bool b_chassis_with_pushbutton;
 
 void init_sm_esc()
 {
     e_stateESC = ESC_DISABLED;
+    if(chassisMode.read()){
+        b_chassis_with_pushbutton = true;
+        }
+        else
+        {
+            b_chassis_with_pushbutton = false;
+        }
     return;
 }
 
@@ -24,13 +32,12 @@
             next_state = ESC_COMMAND;
             break;
         case ESC_COMMAND:
-        if(b_UTILS_flag_emergency_stop)
-        {
-            next_state = ESC_DISABLED;
-        }
+            if(b_UTILS_flag_emergency_stop) {
+                next_state = ESC_DISABLED;
+            }
             break;
         default:
-        break;
+            break;
     }
 
     e_stateESC = next_state;
@@ -39,28 +46,32 @@
 
 void output_sm_esc()
 {
-     switch(e_stateESC) {
+    switch(e_stateESC) {
         case ESC_DISABLED:
-            if(b_UTILS_flag_emergency_stop){
+            if(b_UTILS_flag_emergency_stop) {
                 pc.printf("EMERGENCY STOP\r\n");
                 b_UTILS_flag_emergency_stop = false;
-                relay_ESC = 1;
-                wait(0.5);
+                if(b_chassis_with_pushbutton) {
+                    relay_ESC = 1;
+                    wait(0.5);
+                }
                 relay_ESC = 0;
             }
             break;
-            
+
         case ESC_INIT:
             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);
-            
+
             relay_ESC = 1;
+            if(b_chassis_with_pushbutton) {
             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);
@@ -68,20 +79,21 @@
             pwm_ESC.pulsewidth_us(ESC_PULSE_IDLE_US);
             wait(1);
             break;
-            
+
         case ESC_COMMAND:
             wait(1);
-
+            pwm_ESC.pulsewidth_us(1700);
+            wait(0.7);
             pc.printf("send pulse 1620 to esc\r\n");
             pwm_ESC.pulsewidth_us(1620);
             wait(10);
-            
+
             pc.printf("emergency stop flag raised\r\n");
             b_UTILS_flag_emergency_stop = true;
             break;
         default:
-        break;
+            break;
     }
-    
+
     return;
 }
\ No newline at end of file