Diff: main.cpp

Revision:

44:cd485a9baca0

Parent:

43:037b59837599

Child:

45:9dcbd770898e

--- a/main.cpp	Mon Mar 18 22:18:16 2019 +0000
+++ b/main.cpp	Tue Mar 19 00:19:42 2019 +0000
@@ -38,7 +38,8 @@
 uint32_t distance1, distance2;
 int dist1, dist2, status1, status2;
 int packet_queue[4000] = {0};
-int packet_queue_index = 0;
+int packet_queue_index = 2;
+int current_index = 0, send_count = 0, triple_send_count = 0;
 
 const static int cw = 20 / TIME_SCALE;
 const static int time_cycle = TIMESTAMP_FREQ / TIME_SCALE;
@@ -47,7 +48,8 @@
 bool countdown1_triggered = false, countdown2_triggered = false;
 bool step_in = false, step_out = false;
 bool range1_just_triggered = false, range2_just_triggered = false;
-bool master_connected = false;
+bool master_connected = false, connection_triggered = false;
+bool first_loop_run = true, enter_send_loop = true;
  
 const static char     DEVICE_NAME[]        = "OCCUPY-CRICHTON-ST"; 
 static const uint16_t uuid16_list[]        = {GattService::UUID_HEART_RATE_SERVICE};
@@ -122,7 +124,6 @@
             count--;
         }
      }
-    
 }
 
 void create_packets(int encoding[], double& packet1, double& packet2) {
@@ -135,35 +136,55 @@
     packet2 = bin_to_dec(binary2);
 }
 
-void send_packet(int direction, int time) {     
+void send_packet() {     
+    hrmCounter = packet_queue[current_index];
+    hrService->updateHeartRate(hrmCounter);
+    printf("sent packet %i\n", hrmCounter);
+    ::current_index++;
+}
+
+void enqueue_packet(int direction, int time, int packet_queue[]) {  
     int encoding [16] = {0};
     encode_bin(direction, time, encoding);
     double packet1, packet2;
     create_packets(encoding, packet1, packet2);
     int p1 = (int)packet1;
     int p2 = (int)packet2;
+   
+    packet_queue[packet_queue_index] = p1;
+    packet_queue[packet_queue_index + 1] = p2;
     
-    hrmCounter = p1;
-    hrService->updateHeartRate(hrmCounter);
-    printf("sent packet %i\n", hrmCounter);
-    wait(0.5);
-    hrmCounter = p2;
-    hrService->updateHeartRate(hrmCounter);
-    printf("sent packet %i\n", hrmCounter);
-    wait(0.5);
+    printf("enqueued packet %i\n",  packet_queue[packet_queue_index]);
+    printf("enqueued packet %i\n",  packet_queue[packet_queue_index+1]);
+    
+    ::packet_queue_index += 2;
+    if (packet_queue_index == 4) {
+        ::packet_queue_index = 8;
+    }
 }
 
-void enqueue_packet(int direction, int time, int packet_queue[]) {     
-    packet_queue[packet_queue_index] = direction;
-    packet_queue[packet_queue_index + 1] = time;
+void sudo_enqueue_packet(int direction, int time, int packet_queue[]) {  
+    int encoding [16] = {0};
+    encode_bin(direction, time, encoding);
+    double packet1, packet2;
+    create_packets(encoding, packet1, packet2);
+    int p1 = (int)packet1;
+    int p2 = (int)packet2;
+   
+    packet_queue[0] = p1;
+    packet_queue[1] = p2;
     
-    ::packet_queue_index += 2;
+    printf("sudo enqueued packet %i\n",  packet_queue[0]);
+    printf("sudo enqueued packet %i\n",  packet_queue[1]);
 }
 
 void connectionCallback(const Gap::ConnectionCallbackParams_t *) {
     printf("Bluetooth connected at %i\n", current_time);
     ::master_connected = true;
+    ::connection_triggered = false;
+    sudo_enqueue_packet(-1, current_time, packet_queue);
     BLE::Instance(BLE::DEFAULT_INSTANCE).gap().stopAdvertising();
+    wait(5);
 }
  
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *)
@@ -171,6 +192,10 @@
     printf("Bluetooth disconnected at %i\n", current_time);
     ::master_connected = false;
     ::timestamp = time_cycle;
+    ::current_index = 0;
+    ::packet_queue_index = 2;
+    ::send_count = 0;
+    ::triple_send_count = 0;
     BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
 }
  
@@ -201,20 +226,18 @@
     ::timestamp++;
     ::current_time = timestamp / time_cycle;
     
-    if (packet_queue_index != 0 && master_connected) {
-        led = 0;
-        wait(5);
-        led = 1;
-        
-        send_packet(-1, current_time);
+    if (send_count % 3 == 0) {
+        if (enter_send_loop && connection_triggered && (current_index < packet_queue_index) && master_connected) {
+            send_packet();
+        }
+        ::triple_send_count++;
+    }
+    ::send_count++;
     
-        int current_index = 0;
-        while (current_index < packet_queue_index) {
-            send_packet(packet_queue[current_index], packet_queue[current_index+1]);
-            printf("sent direction %i and time %i\n", packet_queue[current_index], packet_queue[current_index+1]);
-            current_index += 2;
-        }
-        ::packet_queue_index = 0;
+    if ((triple_send_count / 4) % 3 == 0) {
+        ::enter_send_loop = true;
+    } else { 
+        ::enter_send_loop = false;
     }
     
     if (countdown1_triggered) {
@@ -238,19 +261,18 @@
     
     if (status1 == VL53L0X_ERROR_NONE) {
         if (dist1 > DIST_MIN && dist1 < DIST_MAX) {
+            ::connection_triggered = true;
             led1 = 0;
-//            printf("Range1 [mm]:            %6ld\r\n", dist1);
+            printf("Range1 [mm]:            %6ld\r\n", dist1);
             if (!countdown1_triggered && !countdown2_triggered) {
                 countdown1_triggered = true;
                 countdown = cw;
             } else if (countdown2_triggered && !range1_just_triggered) {
-//                printf("STEP IN DETECTED\n");
+                printf("STEP IN DETECTED\n");
                 if (master_connected) {
-                    send_packet(1, 0);
+                    enqueue_packet(1, 0, packet_queue);
                 } else {
                     enqueue_packet(1, current_time, packet_queue);
-                    printf("enqueued direction %i\n",  packet_queue[packet_queue_index-2]);
-                    printf("enqueued time %i\n",  packet_queue[packet_queue_index-1]);
                 }
                 countdown2_triggered = false;
             }
@@ -266,19 +288,18 @@
     }
     if (status2 == VL53L0X_ERROR_NONE) {
         if (dist2 > DIST_MIN && dist2 < DIST_MAX) {
+            ::connection_triggered = true;
             led2 = 0;
-//            printf("Range2 [mm]:            %6ld\r\n", dist2);
+            printf("Range2 [mm]:            %6ld\r\n", dist2);
             if (!countdown1_triggered && !countdown2_triggered) {
                 countdown2_triggered = true;
                 countdown = cw;
             } else if (countdown1_triggered && !range2_just_triggered) {
-//                printf("STEP OUT DETECTED\n");
+                printf("STEP OUT DETECTED\n");
                 if (master_connected) {
-                    send_packet(0, 0);
+                    enqueue_packet(0, 0, packet_queue);
                 } else {
                     enqueue_packet(0, current_time, packet_queue);
-                    printf("enqueued direction %i\n",  packet_queue[packet_queue_index-2]);
-                    printf("enqueued time %i\n",  packet_queue[packet_queue_index-1]);
                 }
                 countdown1_triggered = false;
             }
@@ -300,6 +321,11 @@
     range1.init_sensor(range1_addr);
     range2.init_sensor(range2_addr);
     
+    packet_queue[4] = 1;
+    packet_queue[5] = 1;
+    packet_queue[6] = 1;
+    packet_queue[7] = 1;
+    
     BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
     ble.init(bleInitComplete);