Diff: engine.cpp

Revision:

2:7dfc8dd6aab3

Parent:

1:79b1ee0f97ef

--- a/engine.cpp	Mon Aug 31 22:25:57 2015 +0000
+++ b/engine.cpp	Thu Oct 08 13:36:17 2015 +0000
@@ -1,87 +1,107 @@
 #include "Servo.h"
 #include "car_config.hpp"
+#include "net.hpp"
+#include "can.hpp"
 #include "mbed.h"
 #include "rtos.h"
 
-extern can_cmd_engine_t can_cmd_engine;
+/*** DEFINES ***/
+
+//maximum time interval (in milliseconds) during which the
+//engine can be blocked
+#define MAX_BREAKING_INTERVAL (1000/ENGINE_THREAD_PERIOD) 
+
+/*** LOCAL DATA ***/
+
+//steering servo motor
+static Servo steering_servo(HW_STEERING_SERVO);
+
+//engine
+static PwmOut engine_power(HW_ENGINE_ENABLER);
+static DigitalOut dir1(HW_ENGINE_DIR_1);
+static DigitalOut dir2(HW_ENGINE_DIR_2);
+
+/*** LOCAL FUNCTION PROTOTYPES ***/
+
+//stop the engine (without breaking) and set the steering
+//servo at 0°
+void stop_engine();
 
-Servo steering_servo(p21);
-PwmOut engine_power(p22);
-DigitalOut dir1(p23);
-DigitalOut dir2(p24);
+/*** GLOBAL FUNCTIONS ***/
+
+void init_engine() {
+  steering_servo.calibrate(HW_SERVO_RANGE_INIT,
+                           HW_SERVO_ANGLE_INIT);
+  engine_power.period(HW_ENGINE_PERIOD); //100ms
+  engine_power = 0; //off    
+}
+
+void thread_engine (void const *args) {
+  static uint8 prev_breaking = 0; //if it was breaking the previous step
+  static uint8 breaking_interval = 0; //how long it has been breaking
+  while(1) {
+    if (can_msg_is_missing(CAN_MISSING_CMD_ENGINE_ID))
+      stop_engine();
+    else {
+      //if a valid message has been received
+      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);*/
 
-#define MAX_BREAKING_INTERVAL           (1000/ENGINE_THREAD_PERIOD)     //milliseconds
+        //set steering angle
+        uint8 tmp = can_cmd_engine.payload.msg.steering;
+        steering_servo =  tmp >= 100 ? 1.0 : (float)tmp/100.0;
 
-void init_engine ()
-{
-    steering_servo.calibrate(0.0005, 45.0);
+        //if it is breaking
+        if (can_cmd_engine.payload.msg.breaking) {
+          //it was not breaking during the previous step, so start breaking
+          if (prev_breaking == 0) {
+            breaking_interval = 0;
+            prev_breaking = 1;
+          } else if (breaking_interval < MAX_BREAKING_INTERVAL) {
+            //still breaking
+            dir1 = 1;
+            dir2 = 1;
+            breaking_interval++;
+          } else {
+            //time expired. stop breaking (too much current wasted)
+            dir1 = 1;
+            dir2 = 0;
+            engine_power = 0;
+          }
+        } else {
+          //normal execution (not breaking)
+          //deactivate breaking
+          prev_breaking = 0;
+          //set direction
+          if (can_cmd_engine.payload.msg.direction) {
+            dir1 = 1;
+            dir2 = 0;
+          } else {
+            dir1 = 0;
+            dir2 = 1;
+          }
+          //set engine power
+          tmp = can_cmd_engine.payload.msg.power;
+          engine_power = tmp >= 100 ? 1.0 : (float)tmp/100.0;
+        }
+        //set the message as read
+        can_cmd_engine.flag = CAN_FLAG_EMPTY;
+      }
+    }
+    Thread::wait(ENGINE_THREAD_PERIOD);
+  }
+}
 
-    engine_power.period(0.100); //100ms
-    engine_power = 0; //off    
-}
+/*** LOCAL FUNCTIONS ***/
 
 void stop_engine ()
 {
-    engine_power = 0; //off
-    steering_servo = 0; //center
-    printf("MISSING ENGINE!!!\r\n");
+  engine_power = 0; //off
+  steering_servo = 0; //center
+  dir1 = 1;
+  dir2 = 0;
 }
-
-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);
-    }
-}