Diff: sm_esc.cpp

Revision:

1:8faddee0e52f

Child:

2:e9d928fd327a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sm_esc.cpp	Sat Sep 28 22:29:57 2019 +0000
@@ -0,0 +1,81 @@
+#include "sm_esc.h"
+
+PwmOut pwm_ESC(PD_12);
+DigitalOut relay_ESC(PG_0);
+E_STATE_ESC e_stateESC;
+
+void init_sm_esc()
+{
+    e_stateESC = ESC_DISABLED;
+    return;
+}
+
+void update_sm_esc()
+{
+    E_STATE_ESC next_state;
+
+    switch(e_stateESC) {
+        case ESC_DISABLED:
+            if(b_UTILS_flag_button) {
+                next_state = ESC_INIT;
+                b_UTILS_flag_button = false;
+            }
+            break;
+        case ESC_INIT:
+            next_state = ESC_COMMAND;
+            break;
+        case ESC_COMMAND:
+        if(b_UTILS_flag_emergency_stop)
+        {
+            next_state = ESC_DISABLED;
+        }
+            break;
+        default:
+        break;
+    }
+
+    e_stateESC = next_state;
+    return;
+}
+
+void output_sm_esc()
+{
+     switch(e_stateESC) {
+        case ESC_DISABLED:
+            relay_ESC = 0;
+            break;
+        case ESC_INIT:
+            relay_ESC = 1;
+            wait(0.2);
+            pwm_ESC.period_ms(ESC_PERIOD_DURATION_MS);          //20 ms is default
+            pwm_ESC.pulsewidth_us(600);
+            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");
+            pwm_ESC.pulsewidth_us(1600);
+            wait(1);
+            pc.printf("send pulse 1700 to esc");
+            pwm_ESC.pulsewidth_us(1700);
+            wait(1);
+            pc.printf("send pulse 1800 to esc");
+            pwm_ESC.pulsewidth_us(1800);
+            wait(1);
+            pc.printf("send pulse 1900 to esc");
+            pwm_ESC.pulsewidth_us(1900);
+            wait(1);
+            pc.printf("send pulse 2000 to esc");
+            pwm_ESC.pulsewidth_us(2000);
+            wait(1);
+            pc.printf("emergency stop");
+            b_UTILS_flag_emergency_stop = true;
+            break;
+        default:
+        break;
+    }
+    
+    return;
+}
\ No newline at end of file