main.cpp

00001 #include "mbed.h"
00002 
00003 #include "rtwtypes.h"
00004 #include "serialdata.h"
00005 #include "CircularBuffer.h"
00006 #include "BBSerial.h"
00007 
00008 
00009 // Comment MASTER or SLAVE
00010 #define MASTER
00011 //#define SLAVE
00012 
00013 enum color {
00014     NONE, RED, GREEN, BLUE, LED_M, LED_S
00015 };
00016 
00017 enum {
00018     LED_OFF = 1,
00019     LED_ON = 0
00020 };
00021 
00022 
00023 struct AEBData{
00024     AEBData() : distance(1), fault(0), brake(0) {
00025     }
00026     float distance;
00027     float fault;
00028     float brake;  
00029 };
00030 
00031 DigitalOut red(LED_RED);
00032 DigitalOut green(LED_GREEN);
00033 DigitalOut blue(LED_BLUE);
00034 DigitalOut      trigger(D2);
00035 InterruptIn     echo(D4);
00036 Timer           t;
00037 Ticker          scheduler;
00038 AEBData aebData;
00039 
00040 #ifdef MASTER
00041 AEBData slaveData;
00042 #endif
00043 
00044 
00045 Serial  pc(USBTX, USBRX); // tx, rx
00046 BBSerial boardSerial;
00047 
00048 #include <stddef.h>
00049 #include <stdio.h>                     /* This ert_main.c example uses printf/fflush */
00050 #include "AEB0.h"                      /* Model's header file */
00051 #include "rtwtypes.h"
00052 
00053 /*
00054  * Associating rt_OneStep with a real-time clock or interrupt service routine
00055  * is what makes the generated code "real-time".  The function rt_OneStep is
00056  * always associated with the base rate of the model.  Subrates are managed
00057  * by the base rate from inside the generated code.  Enabling/disabling
00058  * interrupts and floating point context switches are target specific.  This
00059  * example code indicates where these should take place relative to executing
00060  * the generated code step function.  Overrun behavior should be tailored to
00061  * your application needs.  This example simply sets an error status in the
00062  * real-time model and returns from rt_OneStep.
00063  */
00064 void rt_OneStep(void);
00065 void rt_OneStep(void)
00066 {
00067   static boolean_T OverrunFlag = false;
00068 
00069   /* Disable interrupts here */
00070 
00071   /* Check for overrun */
00072   if (OverrunFlag) {
00073     rtmSetErrorStatus(AEB0_M, "Overrun");
00074     return;
00075   }
00076 
00077   OverrunFlag = true;
00078 
00079   /* Save FPU context here (if necessary) */
00080   /* Re-enable timer or interrupt here */
00081   /* Set model inputs here */
00082 
00083   /* Step the model */
00084   AEB0_step();
00085 
00086   /* Get model outputs here */
00087 
00088   /* Indicate task complete */
00089   OverrunFlag = false;
00090 
00091   /* Disable interrupts here */
00092   /* Restore FPU context here (if necessary) */
00093   /* Enable interrupts here */
00094 }
00095 
00096     
00097 void start( void )
00098 {
00099     t.start();
00100 }
00101 
00102 //CircularBuffer distanceCircularBuffer(5);
00103 
00104 void stop( void )
00105 {
00106     t.stop();
00107     float distance_cm = t.read_us() * 343/20000;
00108     if(distance_cm > 50) {
00109         distance_cm = 50;
00110     }
00111     //distanceCircularBuffer.push_back(distance_cm);
00112     aebData.distance = distance_cm;
00113     t.reset();
00114 }
00115 
00116  
00117 extern ExtU_AEB0_T AEB0_U;
00118 extern ExtY_AEB0_T AEB0_Y;
00119 
00120 
00121 
00122 void setColor(color c) {
00123     red = LED_OFF;
00124     blue = LED_OFF;
00125     green = LED_OFF;
00126     
00127     switch(c) {
00128         case NONE:
00129             break;
00130         case RED:
00131             red = LED_ON;
00132             break;
00133         case BLUE:
00134             blue = LED_ON;
00135             break;
00136         case GREEN:
00137             green = LED_ON;
00138         case LED_M:
00139             green = LED_ON;
00140             red = LED_ON;
00141             break;
00142         case LED_S:
00143             blue = LED_ON;
00144             red = LED_ON;
00145             break;
00146         default:
00147             break;
00148     }
00149 }
00150 
00151     
00152 void    do_step( void )
00153 {
00154     #ifdef MASTER
00155     // Get data from simulink
00156     float speed = serialRecv();
00157     // Forward data to slave
00158     // boardSerial.printf("%f %f", aebData.fault, speed);
00159     #elif defined SLAVE
00160     float speed;
00161     float masterFault;
00162     if(boardSerial.scanf("%f %f", &masterFault, &speed) != 2) {
00163         masterFault = 1;
00164         speed = 0;
00165     }
00166     #endif 
00167     
00168     
00169     //Update AEB data
00170     AEB0_U.speed_km_h = speed;
00171     AEB0_U.distance_m = aebData.distance;    
00172  
00173     rt_OneStep();
00174     
00175     aebData.brake = AEB0_Y.brake;
00176     float activateBrake = aebData.brake;
00177     
00178     #ifdef SLAVE
00179     activateBrake = activateBrake && masterFault;
00180     #endif
00181     
00182     if(activateBrake > 0) {
00183         setColor(BLUE);
00184     } else {
00185         #ifdef MASTER
00186         setColor(LED_M);
00187         #else
00188         setColor(LED_S);
00189         #endif
00190     }
00191 
00192     aebData.fault = AEB0_Y.fault;
00193     if(aebData.fault) {
00194         setColor(RED);
00195     }
00196 
00197             
00198     #ifdef MASTER
00199     AEBData slaveData;
00200    // if(boardSerial.scanf("%f %f %f", slaveData.brake, 
00201 //              slaveData.distance, slaveData.fault) != 3) {
00202 //        slaveData.brake = 0;
00203 //        slaveData.distance = 0;
00204 //        slaveData.fault = 1;
00205 //    }
00206     
00207     slaveData.brake = slaveData.distance = slaveData.fault = 0;
00208             
00209               
00210     float data[6] = {aebData.brake, aebData.distance, aebData.fault,
00211                      slaveData.brake, slaveData.distance, slaveData.fault};    
00212     serialSendVec(data, 6);
00213     #elif defined SLAVE
00214     boardSerial.printf("%f %f %f", activateBrake, aebData.distance, aebData.fault);
00215     #endif
00216 }
00217 
00218 
00219 int main()
00220 {
00221     
00222     AEB0_initialize();
00223   
00224     scheduler.attach( &do_step, 0.1 );
00225     
00226     setColor(NONE);
00227     
00228     t.reset();
00229     echo.rise( &start );
00230     echo.fall( &stop );
00231 
00232     trigger = 0;
00233       
00234     while (true) {
00235         trigger = 1;
00236         wait_us( 10 );
00237         trigger = 0;
00238 
00239     }
00240     
00241     AEB0_terminate();
00242     return 0;
00243 }