Diff: main.cpp

Revision:

4:f0be0d8a0394

Parent:

3:4bb49a5dfa47

Child:

5:d64e042b573d

--- a/main.cpp	Sat Jul 30 13:58:52 2016 +0000
+++ b/main.cpp	Sat Jul 30 17:34:13 2016 +0000
@@ -1,7 +1,6 @@
 #include "mbed.h"
 
 #include "rtwtypes.h"
-#include "serialdata.h"
 #include "CircularBuffer.h"
 #include "BBSerial.h"
 
@@ -31,11 +30,18 @@
 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;
@@ -96,6 +102,7 @@
     
 void start( void )
 {
+    timerCounting = 1;
     t.start();
 }
 
@@ -104,9 +111,13 @@
 void stop( void )
 {
     t.stop();
-    float distance_cm = t.read_us() * 343/20000;
+    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;
@@ -117,8 +128,6 @@
 extern ExtU_AEB0_T AEB0_U;
 extern ExtY_AEB0_T AEB0_Y;
 
-
-
 void setColor(color c) {
     red = LED_OFF;
     blue = LED_OFF;
@@ -148,21 +157,19 @@
     }
 }
 
+
+int speed;
+
+#ifdef SLAVE
+   float masterFault = 0;
+#endif
     
 void    do_step( void )
 {
+    
     #ifdef MASTER
-    // Get data from simulink
-    float speed = serialRecv();
-    // Forward data to slave
-    // boardSerial.printf("%f %f", aebData.fault, speed);
+    boardSerial.printf("%f", aebData.fault);
     #elif defined SLAVE
-    float speed;
-    float masterFault;
-    if(boardSerial.scanf("%f %f", &masterFault, &speed) != 2) {
-        masterFault = 1;
-        speed = 0;
-    }
     #endif 
     
     
@@ -170,6 +177,10 @@
     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;
@@ -188,31 +199,14 @@
         setColor(LED_S);
         #endif
     }
-
     aebData.fault = AEB0_Y.fault;
     if(aebData.fault) {
         setColor(RED);
+        #ifdef MASTER
+        masterAliveOut = 0;
+        #endif
     }
-
-            
-    #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
+   
 }
 
 
@@ -220,8 +214,6 @@
 {
     
     AEB0_initialize();
-  
-    scheduler.attach( &do_step, 0.1 );
     
     setColor(NONE);
     
@@ -229,15 +221,34 @@
     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();
+        #endif
         trigger = 1;
         wait_us( 10 );
         trigger = 0;
-
     }
     
     AEB0_terminate();
     return 0;
-}
\ No newline at end of file
+}