engine.cpp

00001 #include "Servo.h"
00002 #include "car_config.hpp"
00003 #include "net.hpp"
00004 #include "can.hpp"
00005 #include "mbed.h"
00006 #include "rtos.h"
00007 
00008 /*** DEFINES ***/
00009 
00010 //maximum time interval (in milliseconds) during which the
00011 //engine can be blocked
00012 #define MAX_BREAKING_INTERVAL (1000/ENGINE_THREAD_PERIOD) 
00013 
00014 /*** LOCAL DATA ***/
00015 
00016 //steering servo motor
00017 static Servo steering_servo(HW_STEERING_SERVO);
00018 
00019 //engine
00020 static PwmOut engine_power(HW_ENGINE_ENABLER);
00021 static DigitalOut dir1(HW_ENGINE_DIR_1);
00022 static DigitalOut dir2(HW_ENGINE_DIR_2);
00023 
00024 /*** LOCAL FUNCTION PROTOTYPES ***/
00025 
00026 //stop the engine (without breaking) and set the steering
00027 //servo at 0°
00028 void stop_engine();
00029 
00030 /*** GLOBAL FUNCTIONS ***/
00031 
00032 void init_engine() {
00033   steering_servo.calibrate(HW_SERVO_RANGE_INIT,
00034                            HW_SERVO_ANGLE_INIT);
00035   engine_power.period(HW_ENGINE_PERIOD); //100ms
00036   engine_power = 0; //off    
00037 }
00038 
00039 void thread_engine (void const *args) {
00040   static uint8 prev_breaking = 0; //if it was breaking the previous step
00041   static uint8 breaking_interval = 0; //how long it has been breaking
00042   while(1) {
00043     if (can_msg_is_missing(CAN_MISSING_CMD_ENGINE_ID))
00044       stop_engine();
00045     else {
00046       //if a valid message has been received
00047       if (can_cmd_engine.flag == CAN_FLAG_RECEIVED) {
00048 /*        printf("ENGINE: %d %d %d %d\r\n",
00049                can_cmd_engine.payload.msg.steering, 
00050                can_cmd_engine.payload.msg.power,
00051                can_cmd_engine.payload.msg.breaking,
00052                can_cmd_engine.payload.msg.direction);*/
00053 
00054         //set steering angle
00055         uint8 tmp = can_cmd_engine.payload.msg.steering;
00056         steering_servo =  tmp >= 100 ? 1.0 : (float)tmp/100.0;
00057 
00058         //if it is breaking
00059         if (can_cmd_engine.payload.msg.breaking) {
00060           //it was not breaking during the previous step, so start breaking
00061           if (prev_breaking == 0) {
00062             breaking_interval = 0;
00063             prev_breaking = 1;
00064           } else if (breaking_interval < MAX_BREAKING_INTERVAL) {
00065             //still breaking
00066             dir1 = 1;
00067             dir2 = 1;
00068             breaking_interval++;
00069           } else {
00070             //time expired. stop breaking (too much current wasted)
00071             dir1 = 1;
00072             dir2 = 0;
00073             engine_power = 0;
00074           }
00075         } else {
00076           //normal execution (not breaking)
00077           //deactivate breaking
00078           prev_breaking = 0;
00079           //set direction
00080           if (can_cmd_engine.payload.msg.direction) {
00081             dir1 = 1;
00082             dir2 = 0;
00083           } else {
00084             dir1 = 0;
00085             dir2 = 1;
00086           }
00087           //set engine power
00088           tmp = can_cmd_engine.payload.msg.power;
00089           engine_power = tmp >= 100 ? 1.0 : (float)tmp/100.0;
00090         }
00091         //set the message as read
00092         can_cmd_engine.flag = CAN_FLAG_EMPTY;
00093       }
00094     }
00095     Thread::wait(ENGINE_THREAD_PERIOD);
00096   }
00097 }
00098 
00099 /*** LOCAL FUNCTIONS ***/
00100 
00101 void stop_engine ()
00102 {
00103   engine_power = 0; //off
00104   steering_servo = 0; //center
00105   dir1 = 1;
00106   dir2 = 0;
00107 }