File content as of revision 3:4bb49a5dfa47:

#include "mbed.h"

#include "rtwtypes.h"
#include "serialdata.h"
#include "CircularBuffer.h"
#include "BBSerial.h"


// Comment MASTER or SLAVE
#define MASTER
//#define SLAVE

enum color {
    NONE, RED, GREEN, BLUE, LED_M, LED_S
};

enum {
    LED_OFF = 1,
    LED_ON = 0
};


struct AEBData{
    AEBData() : distance(1), fault(0), brake(0) {
    }
    float distance;
    float fault;
    float brake;  
};

DigitalOut red(LED_RED);
DigitalOut green(LED_GREEN);
DigitalOut blue(LED_BLUE);
DigitalOut      trigger(D2);
InterruptIn     echo(D4);
Timer           t;
Ticker          scheduler;
AEBData aebData;

#ifdef MASTER
AEBData slaveData;
#endif


Serial  pc(USBTX, USBRX); // tx, rx
BBSerial boardSerial;

#include <stddef.h>
#include <stdio.h>                     /* This ert_main.c example uses printf/fflush */
#include "AEB0.h"                      /* Model's header file */
#include "rtwtypes.h"

/*
 * Associating rt_OneStep with a real-time clock or interrupt service routine
 * is what makes the generated code "real-time".  The function rt_OneStep is
 * always associated with the base rate of the model.  Subrates are managed
 * by the base rate from inside the generated code.  Enabling/disabling
 * interrupts and floating point context switches are target specific.  This
 * example code indicates where these should take place relative to executing
 * the generated code step function.  Overrun behavior should be tailored to
 * your application needs.  This example simply sets an error status in the
 * real-time model and returns from rt_OneStep.
 */
void rt_OneStep(void);
void rt_OneStep(void)
{
  static boolean_T OverrunFlag = false;

  /* Disable interrupts here */

  /* Check for overrun */
  if (OverrunFlag) {
    rtmSetErrorStatus(AEB0_M, "Overrun");
    return;
  }

  OverrunFlag = true;

  /* Save FPU context here (if necessary) */
  /* Re-enable timer or interrupt here */
  /* Set model inputs here */

  /* Step the model */
  AEB0_step();

  /* Get model outputs here */

  /* Indicate task complete */
  OverrunFlag = false;

  /* Disable interrupts here */
  /* Restore FPU context here (if necessary) */
  /* Enable interrupts here */
}

    
void start( void )
{
    t.start();
}

//CircularBuffer distanceCircularBuffer(5);

void stop( void )
{
    t.stop();
    float distance_cm = t.read_us() * 343/20000;
    if(distance_cm > 50) {
        distance_cm = 50;
    }
    //distanceCircularBuffer.push_back(distance_cm);
    aebData.distance = distance_cm;
    t.reset();
}

 
extern ExtU_AEB0_T AEB0_U;
extern ExtY_AEB0_T AEB0_Y;



void setColor(color c) {
    red = LED_OFF;
    blue = LED_OFF;
    green = LED_OFF;
    
    switch(c) {
        case NONE:
            break;
        case RED:
            red = LED_ON;
            break;
        case BLUE:
            blue = LED_ON;
            break;
        case GREEN:
            green = LED_ON;
        case LED_M:
            green = LED_ON;
            red = LED_ON;
            break;
        case LED_S:
            blue = LED_ON;
            red = LED_ON;
            break;
        default:
            break;
    }
}

    
void    do_step( void )
{
    #ifdef MASTER
    // Get data from simulink
    float speed = serialRecv();
    // Forward data to slave
    // boardSerial.printf("%f %f", aebData.fault, speed);
    #elif defined SLAVE
    float speed;
    float masterFault;
    if(boardSerial.scanf("%f %f", &masterFault, &speed) != 2) {
        masterFault = 1;
        speed = 0;
    }
    #endif 
    
    
    //Update AEB data
    AEB0_U.speed_km_h = speed;
    AEB0_U.distance_m = aebData.distance;    
 
    rt_OneStep();
    
    aebData.brake = AEB0_Y.brake;
    float activateBrake = aebData.brake;
    
    #ifdef SLAVE
    activateBrake = activateBrake && masterFault;
    #endif
    
    if(activateBrake > 0) {
        setColor(BLUE);
    } else {
        #ifdef MASTER
        setColor(LED_M);
        #else
        setColor(LED_S);
        #endif
    }

    aebData.fault = AEB0_Y.fault;
    if(aebData.fault) {
        setColor(RED);
    }

            
    #ifdef MASTER
    AEBData slaveData;
   // if(boardSerial.scanf("%f %f %f", slaveData.brake, 
//              slaveData.distance, slaveData.fault) != 3) {
//        slaveData.brake = 0;
//        slaveData.distance = 0;
//        slaveData.fault = 1;
//    }
    
    slaveData.brake = slaveData.distance = slaveData.fault = 0;
            
              
    float data[6] = {aebData.brake, aebData.distance, aebData.fault,
                     slaveData.brake, slaveData.distance, slaveData.fault};    
    serialSendVec(data, 6);
    #elif defined SLAVE
    boardSerial.printf("%f %f %f", activateBrake, aebData.distance, aebData.fault);
    #endif
}


int main()
{
    
    AEB0_initialize();
  
    scheduler.attach( &do_step, 0.1 );
    
    setColor(NONE);
    
    t.reset();
    echo.rise( &start );
    echo.fall( &stop );

    trigger = 0;
      
    while (true) {
        trigger = 1;
        wait_us( 10 );
        trigger = 0;

    }
    
    AEB0_terminate();
    return 0;
}