Diff: engine.cpp

Revision:

1:79b1ee0f97ef

Parent:

0:ce6055872f4e

Child:

2:7dfc8dd6aab3

--- a/engine.cpp	Sun Aug 02 12:55:33 2015 +0000
+++ b/engine.cpp	Mon Aug 31 22:25:57 2015 +0000
@@ -1,38 +1,87 @@
-//#include "Servo.h"
+#include "Servo.h"
+#include "car_config.hpp"
+#include "mbed.h"
+#include "rtos.h"
 
-/*
-void thread_ecm (void const *args)
-{
-    //read signals
-    float power = read_engine_power();             //[0, 100]
-    uint8 direction = read_engine_direction();     //[0, 1]
-    float angle = read_steering_angle();           //[0, 100]
-    //use them
-    if ((power>=0.0) && (power<=100.0))
-        engine_power = power/100.0;
-    if ((direction==0) || (direction==1))
-        engine_direction = direction;
-    if ((angle<=100.0) && (angle>=0.0))
-        steering_servo = angle/100.0;        
-    //wait
-    Thread::wait(100);
-}
-*/
-
-
-/*
+extern can_cmd_engine_t can_cmd_engine;
 
 Servo steering_servo(p21);
 PwmOut engine_power(p22);
+DigitalOut dir1(p23);
+DigitalOut dir2(p24);
 
-void thread_steering (void const *args);
-void thread_engine (void const *args);
+#define MAX_BREAKING_INTERVAL           (1000/ENGINE_THREAD_PERIOD)     //milliseconds
 
-void init_hw ()
+void init_engine ()
 {
     steering_servo.calibrate(0.0005, 45.0);
-    
-    engine_power.period(0.020); //20ms
+
+    engine_power.period(0.100); //100ms
+    engine_power = 0; //off    
+}
+
+void stop_engine ()
+{
     engine_power = 0; //off
+    steering_servo = 0; //center
+    printf("MISSING ENGINE!!!\r\n");
 }
-*/
+
+void thread_engine (void const *args)
+{
+    static uint8 prev_direction = 0;
+    static uint8 prev_breaking = 0;
+    static uint8 breaking_interval = 0;
+    while(1) {
+        //printf("ENGINE\r\n");
+        if (can_msg_is_missing(CAN_MISSING_ENGINE_ID))
+            stop_engine();
+        else {
+            if (can_cmd_engine.flag == CAN_FLAG_RECEIVED) {
+                printf("ENGINE: %d %d %d %d\r\n", can_cmd_engine.payload.msg.steering, 
+                                                     can_cmd_engine.payload.msg.power,
+                                                     can_cmd_engine.payload.msg.breaking,
+                                                     can_cmd_engine.payload.msg.direction);
+                uint8 tmp = can_cmd_engine.payload.msg.steering;
+                if (tmp>100)
+                    tmp = 100;
+                steering_servo = (float)tmp/100.0;
+
+                if (can_cmd_engine.payload.msg.breaking) {
+                    if (prev_breaking == 0) {
+                        breaking_interval = 0;
+                        prev_breaking = 1;
+                    }
+                    if (breaking_interval < MAX_BREAKING_INTERVAL) {
+                        dir1 = 1;
+                        dir2 = 1;
+                        breaking_interval++;
+                    } else {
+                        dir1 = 1;
+                        dir2 = 0;
+                    }
+                    engine_power = 0;
+                } else {
+                    prev_breaking = 0;
+                    if (prev_direction != can_cmd_engine.payload.msg.direction)
+                    {
+                        dir1 = 0;
+                        dir2 = 0;
+                        if (can_cmd_engine.payload.msg.direction)
+                            dir1 = 1;
+                        else
+                            dir2 = 1;
+                        prev_direction = can_cmd_engine.payload.msg.direction;
+                    }
+                    tmp = can_cmd_engine.payload.msg.power;
+
+                    if (tmp<100)
+                        tmp = 100;
+                    engine_power = (float)tmp/100.0;
+                }
+                can_cmd_engine.flag = CAN_FLAG_EMPTY;
+            }
+        }
+        Thread::wait(ENGINE_THREAD_PERIOD);
+    }
+}