main.cpp

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