Diff: main.cpp

Revision:

2:5811e080f41d

Parent:

1:45911e86ffee

Child:

3:4bb49a5dfa47

--- a/main.cpp	Tue Jul 26 20:15:23 2016 +0000
+++ b/main.cpp	Sat Jul 30 08:23:21 2016 +0000
@@ -2,6 +2,7 @@
 
 #include "rtwtypes.h"
 #include "serialdata.h"
+#include "CircularBuffer.h"
 
 DigitalOut red(LED_RED);
 DigitalOut green(LED_GREEN);
@@ -82,12 +83,19 @@
     t.start();
 }
 
+CircularBuffer distanceCircularBuffer(5);
+
+float sensDist = 1;
+
 void stop( void )
 {
     t.stop();
-    distance = t.read_us()/58.0;
-    distance /= 2; // from 0 to 45m distance;
-    if(distance>150) distance = 150;
+    float distance_cm = t.read_us() * 343/20000;
+    if(distance_cm > 50) {
+        distance_cm = 50;
+    }
+    //distanceCircularBuffer.push_back(distance_cm);
+    sensDist = distance_cm;
     t.reset();
 }
 
@@ -100,14 +108,15 @@
 };
 
 enum {
-    LED_ON = 0,
-    LED_OFF = 1
+    LED_OFF = 1,
+    LED_ON = 0
 };
 
 void setColor(color c) {
     red = LED_OFF;
     blue = LED_OFF;
     green = LED_OFF;
+    
     switch(c) {
         case NONE:
             break;
@@ -123,30 +132,42 @@
     }
 }
 
+
+
+float brake; 
+
+    
 void    do_step( void )
 {
-    AEB0_U.speed_ms = serialRecv();
-    AEB0_U.distance_m = distance;
-//    
+    AEB0_U.speed_km_h = serialRecv();
+    AEB0_U.distance_m = sensDist;    
  
     rt_OneStep();
-    float brake = AEB0_Y.brake;
+    brake = AEB0_Y.brake;
+    if(brake > 0) {
+        setColor(BLUE);
+    } else {
+        setColor(NONE);
+    }
+
     fault = AEB0_Y.fault;
     if(fault) {
         setColor(RED);
     }
-    float data[3] = {brake, distance, fault};
+    float data[3] = {brake, sensDist, fault};
     serialSendVec(data, 3);
 }
-    
+
+
 int main()
 {
     
     AEB0_initialize();
   
-    scheduler.attach( &do_step, 0.2 );
+    scheduler.attach( &do_step, 0.1 );
+    
     setColor(NONE);
-    //
+    
     t.reset();
     echo.rise( &start );
     echo.fall( &stop );