File content as of revision 1:79b1ee0f97ef:

#include "Servo.h"
#include "car_config.hpp"
#include "mbed.h"
#include "rtos.h"

extern can_cmd_engine_t can_cmd_engine;

Servo steering_servo(p21);
PwmOut engine_power(p22);
DigitalOut dir1(p23);
DigitalOut dir2(p24);

#define MAX_BREAKING_INTERVAL           (1000/ENGINE_THREAD_PERIOD)     //milliseconds

void init_engine ()
{
    steering_servo.calibrate(0.0005, 45.0);

    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);
    }
}