Revision 3:4bb49a5dfa47, committed 2016-07-30

Comitter:

clynamen

Date:

Sat Jul 30 13:58:52 2016 +0000

Parent:

2:5811e080f41d

Commit message:

m;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BBSerial.cpp	Sat Jul 30 13:58:52 2016 +0000
@@ -0,0 +1,60 @@
+#include "BBSerial.h"
+
+#include "stdio.h"
+
+BBSerial::BBSerial() :
+uart(PTC17, PTC16) // tx, rx of uart3
+{
+    uart.baud(9600);
+    uart.format(8, SerialBase::Odd, 2);
+}
+
+
+void BBSerial::printf(const char* format, ...) {
+    va_list argptr;
+    va_start(argptr, format);
+    
+    char newFormat[200];
+    
+    // Add garbage and header in order to resolve first byte error
+    sprintf(newFormat, "GARBAGEAAAAA%sZZZZZ\n", format);
+    char buf[200];
+    vsprintf(buf, newFormat, argptr);
+    uart.printf("%s", buf);
+ 
+    va_end(argptr);   
+}
+
+int BBSerial::scanf(const char* format, ...) {
+    if(!uart.readable()) {
+        return 0;
+    }
+    va_list argptr;
+    va_start(argptr, format);
+    
+    char line[200];
+    uart.gets(line, 200);
+    line[199] = '\0';
+    
+    int read=0;
+    
+    int start = 0;
+    for (int i = 0; i < 10; i++) {
+        if(strncmp("AAAAA", &line[i], 5) == 0) {
+            start = i+5;
+        }
+    }
+    char *clean = &line[start];
+    
+    int end = strlen(clean)-6;
+    clean[end] = 0;
+    
+    if(start < 1) {
+        read = 0;
+    } else {
+        read = vsscanf(clean, format, argptr);
+    }
+      
+    va_end(argptr);   
+    return read;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BBSerial.h	Sat Jul 30 13:58:52 2016 +0000
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <mbed.h>
+
+// Handle an error-correction connection between two boards
+// Board to Board serial
+// using UART3
+class BBSerial {
+
+    
+    public:
+        
+         BBSerial();
+        void printf(const char* format, ...);
+        int scanf(const char* format, ...);
+    
+    private:
+        
+    
+    Serial  uart;
+};
\ No newline at end of file

--- 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();