File content as of revision 5:d64e042b573d:

#include "mbed.h"

#include "rtwtypes.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);
#ifdef MASTER
DigitalOut masterAliveOut(D7);
#else 
DigitalIn masterAliveIn(D7, PullDown);
#endif 
DigitalOut      trigger(D2);
InterruptIn     echo(D4);
Timer           t;
bool timerCounting = false;
Ticker          scheduler;
AEBData aebData;
float ddd;

#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 )
{
    timerCounting = 1;
    t.start();
}

//CircularBuffer distanceCircularBuffer(5);

void stop( void )
{
    t.stop();
    timerCounting = 0;
    float distance_cm = t.read_us() * 343.0f/20000;
    ddd = distance_cm;
    if(distance_cm > 50) {
        distance_cm = 50;
    } else if (distance_cm < 2) {
        distance_cm = 0;
    }
    //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;
    }
}


int speed;

#ifdef SLAVE
   float masterFault = 0;
#endif
    
void    do_step( void )
{
    
    #ifdef MASTER
    boardSerial.printf("%f", aebData.fault);
    #elif defined SLAVE
    #endif 
    
    
    //Update AEB data
    AEB0_U.speed_km_h = speed;
    AEB0_U.distance_m = aebData.distance;    
 
    pc.printf("do step, distance: %f\r\n", aebData.distance);
    #ifdef SLAVE
    pc.printf("master fault: %f\r\n", masterFault);
    #endif
    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
        masterAliveOut = 0;
        #endif
    }
   
}


int main()
{
    
    AEB0_initialize();
    
    setColor(NONE);
    
    t.reset();
    echo.rise( &start );
    echo.fall( &stop );


    trigger = 0;
    #ifdef MASTER
        pc.puts("MASTER\r\n");
        masterAliveOut = 1;
    #else
        pc.puts("SLAVE\r\n");
    #endif
    pc.puts("insert speed value\r\n");
    
    char buf[200];
    pc.gets(buf, 200);
    buf[199] = 0;
    sscanf(buf, "%d", &speed);
    pc.printf("\r\nspeed was: %d\r\n", speed);
   
    
    scheduler.attach( &do_step, 0.1 );
      
    while (true) {
        #ifdef SLAVE
        masterFault = !masterAliveIn.read();
        pc.printf("\r\nmaster fault: %f\r\n", masterFault);
        #endif
        trigger = 1;
        wait_us( 10 );
        trigger = 0;
    }
    
    AEB0_terminate();
    return 0;
}