Diff: main.cpp

Revision:

3:4bb49a5dfa47

Parent:

2:5811e080f41d

Child:

4:f0be0d8a0394

--- a/main.cpp	Sat Jul 30 08:23:21 2016 +0000
+++ b/main.cpp	Sat Jul 30 13:58:52 2016 +0000
@@ -3,25 +3,47 @@
 #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;
 
-volatile float           distance;
-float fault = 0;
+#ifdef MASTER
+AEBData slaveData;
+#endif
+
 
 Serial  pc(USBTX, USBRX); // tx, rx
-
-
-// 
-// Copy from ert_main.c
-//
+BBSerial boardSerial;
 
 #include <stddef.h>
 #include <stdio.h>                     /* This ert_main.c example uses printf/fflush */
@@ -71,21 +93,13 @@
   /* Enable interrupts here */
 }
 
-  
-//
-// End copy
-//
-
-
     
-void    start( void )
+void start( void )
 {
     t.start();
 }
 
-CircularBuffer distanceCircularBuffer(5);
-
-float sensDist = 1;
+//CircularBuffer distanceCircularBuffer(5);
 
 void stop( void )
 {
@@ -95,7 +109,7 @@
         distance_cm = 50;
     }
     //distanceCircularBuffer.push_back(distance_cm);
-    sensDist = distance_cm;
+    aebData.distance = distance_cm;
     t.reset();
 }
 
@@ -103,14 +117,7 @@
 extern ExtU_AEB0_T AEB0_U;
 extern ExtY_AEB0_T AEB0_Y;
 
-enum color {
-    NONE, RED, GREEN, BLUE
-};
 
-enum {
-    LED_OFF = 1,
-    LED_ON = 0
-};
 
 void setColor(color c) {
     red = LED_OFF;
@@ -128,34 +135,84 @@
             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;
     }
 }
 
-
-
-float brake; 
-
     
 void    do_step( void )
 {
-    AEB0_U.speed_km_h = serialRecv();
-    AEB0_U.distance_m = sensDist;    
+    #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();
-    brake = AEB0_Y.brake;
-    if(brake > 0) {
+    
+    aebData.brake = AEB0_Y.brake;
+    float activateBrake = aebData.brake;
+    
+    #ifdef SLAVE
+    activateBrake = activateBrake && masterFault;
+    #endif
+    
+    if(activateBrake > 0) {
         setColor(BLUE);
     } else {
-        setColor(NONE);
+        #ifdef MASTER
+        setColor(LED_M);
+        #else
+        setColor(LED_S);
+        #endif
+    }
+
+    aebData.fault = AEB0_Y.fault;
+    if(aebData.fault) {
+        setColor(RED);
     }
 
-    fault = AEB0_Y.fault;
-    if(fault) {
-        setColor(RED);
-    }
-    float data[3] = {brake, sensDist, fault};
-    serialSendVec(data, 3);
+            
+    #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
 }
 
 
@@ -175,14 +232,10 @@
     trigger = 0;
       
     while (true) {
-      //  pc.printf( "Reading inputs....\n\r" );
-
-
         trigger = 1;
         wait_us( 10 );
         trigger = 0;
 
-
     }
     
     AEB0_terminate();