Diff: main.cpp

Revision:

29:6ba8491c1dab

Parent:

28:ca14866eec64

Child:

30:5e1c85083f24

--- a/main.cpp	Wed Mar 13 15:16:25 2019 +0000
+++ b/main.cpp	Wed Mar 13 17:08:49 2019 +0000
@@ -3,12 +3,20 @@
 #include "VL53L0X.h"
 #include "ble/services/HeartRateService.h"
 
-#define range1_addr (0x56)
-#define range2_addr (0x60)
-#define range1_XSHUT   p15
-#define range2_XSHUT   p16
-#define VL53L0_I2C_SDA   p30 
-#define VL53L0_I2C_SCL   p7  
+#define range1_addr         (0x56)
+#define range2_addr         (0x60)
+#define range1_XSHUT        p15
+#define range2_XSHUT        p16
+#define VL53L0_I2C_SDA      p30 
+#define VL53L0_I2C_SCL      p7  
+#define TIME_SCALE          2 // sensors activated every 100ms * TIME_SCALE
+#define DIST_MIN            0
+#define DIST_MAX            22
+
+const static int cw = 20 / TIME_SCALE;
+int countdown = cw;
+bool countdown1_triggered = false, countdown2_triggered = false;
+bool step_in = false, step_out = false;
  
 Serial pc(USBTX, USBRX);
 static DevI2C devI2c(VL53L0_I2C_SDA, VL53L0_I2C_SCL); 
@@ -24,12 +32,8 @@
 static DigitalOut shutdown2_pin(range2_XSHUT);
 static VL53L0X range2(&devI2c, &shutdown2_pin, NC);
 
-uint32_t distance1;
-uint32_t distance2;
-int dist1;
-int dist2;
-int status1;
-int status2;
+uint32_t distance1, distance2;
+int dist1, dist2, status1, status2;
 
 const static char     DEVICE_NAME[]        = "OCCUPY-CRICHTON-ST";
 static const uint16_t uuid16_list[]        = {GattService::UUID_HEART_RATE_SERVICE};
@@ -83,6 +87,20 @@
 void wakeup_event_cb() {
     led != led;
     
+    if (countdown1_triggered) {
+        countdown--;
+        if (countdown == 0) {
+            countdown1_triggered = false;
+        }
+    }
+    if (countdown2_triggered) {
+
+        countdown--;
+        if (countdown == 0) {
+            countdown2_triggered = false;
+        }
+    }
+    
     status1 = range1.get_distance(&distance1);
     status2 = range2.get_distance(&distance2);
     
@@ -90,10 +108,19 @@
     dist2 = format_dist(distance2);
 
     if (status1 == VL53L0X_ERROR_NONE) {
-        printf("Range1 [mm]:            %6ld\r\n", dist1);
+//        printf("Range1 [mm]:            %6ld\r\n", dist1);
 
-        if (dist1 > 4 && dist1 < 220) {
+        if (dist1 > DIST_MIN && dist1 < DIST_MAX) {
             led1 = 0;
+            
+            if (!countdown1_triggered && !countdown2_triggered) {
+                countdown1_triggered = true;
+                countdown = cw;
+            } else if (countdown2_triggered) {
+                printf("STEP IN DETECTED\n");
+                countdown2_triggered = false;
+            }
+            
             hrmCounter = dist1;
             hrService->updateHeartRate(hrmCounter);
         }
@@ -101,18 +128,26 @@
             led1 = 1;
         }
     } else {
-        printf("Range1 [mm]:                --\r\n");
+//        printf("Range1 [mm]:                --\r\n");
         led1 = 1;
     }
     if (status2 == VL53L0X_ERROR_NONE) {
-        printf("Range2 [mm]:            %6ld\r\n", dist2);
-        if (dist2 > 4 && dist2 < 220) {
+        if (dist2 > DIST_MIN && dist2 < DIST_MAX) {
             led2 = 0;
+            
+            if (!countdown1_triggered && !countdown2_triggered) {
+                countdown2_triggered = true;
+                countdown = cw;
+            } else if (countdown1_triggered) {
+                printf("STEP OUT DETECTED\n");
+                countdown1_triggered = false;
+            }
+            
             hrmCounter = dist2;
             hrService->updateHeartRate(hrmCounter);
         }
     } else {
-        printf("Range2 [mm]:                --\r\n");
+//        printf("Range2 [mm]:                --\r\n");
         led2 = 1;
     }
 }
@@ -128,7 +163,7 @@
     ble.init(bleInitComplete);
     
     Ticker ticker;
-    ticker.attach(wakeup_event_cb, 0.3);
+    ticker.attach(wakeup_event_cb, 0.1*TIME_SCALE);
     
     while (ble.hasInitialized()) {
         ble.waitForEvent();