Diff: sm_esc.cpp

Revision:

6:ab9f3695633f

Parent:

5:8bbe640528bc

Child:

7:23bb20892c8c

--- a/sm_esc.cpp	Sun Sep 29 17:09:48 2019 +0000
+++ b/sm_esc.cpp	Sun Sep 29 20:07:54 2019 +0000
@@ -1,20 +1,15 @@
 #include "sm_esc.h"
+#include "sm_sections.h"
 
 PwmOut pwm_ESC(PD_12);
 DigitalOut relay_ESC(PG_0);
 E_STATE_ESC e_stateESC;
-bool b_chassis_with_pushbutton;
+Timer t_emergency_brake;
+int pulsewidth_test = 1800;
 
 void init_sm_esc()
 {
     e_stateESC = ESC_DISABLED;
-    if(chassisMode.read()){
-        b_chassis_with_pushbutton = true;
-        }
-        else
-        {
-            b_chassis_with_pushbutton = false;
-        }
     return;
 }
 
@@ -61,14 +56,15 @@
 
         case ESC_INIT:
             pc.printf("Init esc\r\n");
+            odroid.printf("pulsewidth_test: %d\r\n",pulsewidth_test);
             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.5);
+                relay_ESC = 0;
             }
             wait(0.1);
 
@@ -78,18 +74,30 @@
             wait(1);
             pwm_ESC.pulsewidth_us(ESC_PULSE_IDLE_US);
             wait(1);
+            t_emergency_brake.start();
+            t_emergency_brake.reset();
             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;
+            if(t_emergency_brake.read_ms() < 2000) {
+                pwm_ESC.pulsewidth_us(pulsewidth_test);
+            } else {
+                pc.printf("emergency stop flag raised\r\n");
+                b_UTILS_flag_emergency_stop = true;
+                t_emergency_brake.stop();
+                t_emergency_brake.reset();
+            }
+            /*switch(e_stateSections) {
+                case RUNNING_SECTION:
+                    pwm_ESC.pulsewidth_us(1660);
+                    break;
+                case STOPPED:
+                    pc.printf("emergency stop flag raised\r\n");
+                    b_UTILS_flag_emergency_stop = true;
+                    break;
+                default:
+                    break;
+            }*/
             break;
         default:
             break;