Revision 47:eeed074999ab, committed 2019-03-19

Comitter:

vazbyte

Date:

Tue Mar 19 18:25:39 2019 +0000

Parent:

46:a8013e7450b2

Child:

48:78310b56de00

Commit message:

refactored

Changed in this revision

--- a/main.cpp	Tue Mar 19 00:37:33 2019 +0000
+++ b/main.cpp	Tue Mar 19 18:25:39 2019 +0000
@@ -26,6 +26,7 @@
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led(LED3, 1);
+Ticker ticker;
 uint16_t customServiceUUID  = 0xA000;
 uint16_t readCharUUID       = 0xA001;
 uint16_t writeCharUUID      = 0xA002;
@@ -47,7 +48,7 @@
 int countdown = cw;
 bool countdown1_triggered = false, countdown2_triggered = false;
 bool step_in = false, step_out = false;
-bool range1_just_triggered = false, range2_just_triggered = false;
+bool range1_triggered = false, range2_triggered = false;
 bool master_connected = false, connection_triggered = false;
 bool first_loop_run = true, enter_send_loop = true;
  
@@ -58,6 +59,7 @@
  
 HeartRateService         *hrService;
 uint8_t hrmCounter = 0; 
+
 int format_dist(int distance) {
    int result;
    if (distance > 2550)
@@ -67,6 +69,22 @@
    return result; 
 }
 
+void check_for_countdown(bool countdown_triggered) {
+    if (countdown_triggered) {
+        ::countdown--;
+        if (countdown == 0) {
+            countdown_triggered = false;
+        }
+    }
+}
+
+
+void optimize_transmissions(int packet_queue[]) {
+    for (int count = 4; count < 12; count++) {
+        packet_queue[count] = 1;
+    }
+}
+
 void dec_to_bin(int decimal, int bin[]) {
     int temp;
     // encoding little-endian
@@ -143,10 +161,7 @@
     ::current_index++;
 }
 
-void enqueue_packet(int direction, int time, int packet_queue[]) { 
-    if (packet_queue_index == 4) {
-        ::packet_queue_index = 12;
-    } 
+void enqueue_packet(int direction, int time, int packet_queue[], int priority) { 
     int encoding [16] = {0};
     encode_bin(direction, time, encoding);
     double packet1, packet2;
@@ -154,35 +169,70 @@
     int p1 = (int)packet1;
     int p2 = (int)packet2;
    
+    if (priority == 1) {
+        packet_queue[0] = p1;
+        packet_queue[1] = p2;
+    
+        printf("sudo enqueued packet %i\n",  packet_queue[0]);
+        printf("sudo enqueued packet %i\n",  packet_queue[1]);
+    } else {        
+        if (packet_queue_index == 4) {
+            ::packet_queue_index = 12;
+        } 
+        packet_queue[packet_queue_index] = p1;
+        packet_queue[packet_queue_index + 1] = p2;
+    
+        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;
+    }
+}
+
+void enqueue_transmission_break() {
+    int p1 = 0;
+    int p2 = 0;
+    
     packet_queue[packet_queue_index] = p1;
     packet_queue[packet_queue_index + 1] = p2;
-    
-    printf("enqueued packet %i\n",  packet_queue[packet_queue_index]);
-    printf("enqueued packet %i\n",  packet_queue[packet_queue_index+1]);
+    printf("enqueued transmission breaks %i and %i\n", packet_queue[packet_queue_index], packet_queue[packet_queue_index+1]);
     
     ::packet_queue_index += 2;
 }
 
-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;
-    
-    printf("sudo enqueued packet %i\n",  packet_queue[0]);
-    printf("sudo enqueued packet %i\n",  packet_queue[1]);
+void check_for_step(int direction, int status, int dist, DigitalOut led, bool& this_countdown_triggered, 
+                    bool& other_countdown_triggered, bool& this_range_triggered, bool& other_range_triggered,
+                    int packet_queue[]) {
+    if (status == VL53L0X_ERROR_NONE) {
+        if (dist > DIST_MIN && dist < DIST_MAX) {
+            ::connection_triggered = true;
+            led = 0;
+            printf("Range%i [mm]:            %6ld\r\n", direction+1, dist);
+            if (!this_countdown_triggered && !other_countdown_triggered) {
+                this_countdown_triggered = true;
+                ::countdown = cw;
+            } else if (other_countdown_triggered && !this_range_triggered) {
+                printf("STEP %i DETECTED\n", direction);
+                enqueue_packet(direction, current_time, packet_queue, 0);
+                other_countdown_triggered = false;
+            }
+            this_range_triggered = true;
+        } else {
+            led = 1;
+            this_range_triggered = false;
+        }
+    } else {
+        led = 1;
+        other_range_triggered = false;
+    }
 }
 
 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);
+    enqueue_packet(-1, current_time, packet_queue, 1);
+    enqueue_transmission_break();
     BLE::Instance(BLE::DEFAULT_INSTANCE).gap().stopAdvertising();
     wait(5);
 }
@@ -240,18 +290,8 @@
         ::enter_send_loop = false;
     }
     
-    if (countdown1_triggered) {
-        countdown--;
-        if (countdown == 0) {
-            countdown1_triggered = false;
-        }
-    }
-    if (countdown2_triggered) {
-        countdown--;
-        if (countdown == 0) {
-            countdown2_triggered = false;
-        }
-    }
+    check_for_countdown(countdown1_triggered);
+    check_for_countdown(countdown2_triggered);
         
     status1 = range1.get_distance(&distance1);
     status2 = range2.get_distance(&distance2);
@@ -259,61 +299,10 @@
     dist1 = format_dist(distance1);
     dist2 = format_dist(distance2);
     
-    if (status1 == VL53L0X_ERROR_NONE) {
-        if (dist1 > DIST_MIN && dist1 < DIST_MAX) {
-            ::connection_triggered = true;
-            led1 = 0;
-            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");
-//                if (master_connected) {
-//                    enqueue_packet(1, 0, packet_queue);
-//                } else {
-                    enqueue_packet(1, current_time, packet_queue);
-//                }
-                countdown2_triggered = false;
-            }
-            range1_just_triggered = true;
-        }
-        else {
-            led1 = 1;
-            range1_just_triggered = false;
-        }
-    } else {
-        led1 = 1;
-        range2_just_triggered = false;
-    }
-    if (status2 == VL53L0X_ERROR_NONE) {
-        if (dist2 > DIST_MIN && dist2 < DIST_MAX) {
-            ::connection_triggered = true;
-            led2 = 0;
-            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");
-//                if (master_connected) {
-//                    enqueue_packet(0, 0, packet_queue);
-//                } else {
-                    enqueue_packet(0, current_time, packet_queue);
-//                }
-                countdown1_triggered = false;
-            }
-            
-            range2_just_triggered = true;
-        }
-        else {
-            led2 = 1;
-            range2_just_triggered = false;
-        }
-    } else {
-        led2 = 1;
-        range2_just_triggered = false;
-    }
+    check_for_step(1, status1, dist1, led1, countdown1_triggered, countdown2_triggered, 
+                    range1_triggered, range2_triggered, packet_queue);
+    check_for_step(0, status2, dist2, led2, countdown2_triggered, countdown1_triggered, 
+                    range2_triggered, range1_triggered, packet_queue);
 }
  
 int main(void)
@@ -321,14 +310,11 @@
     range1.init_sensor(range1_addr);
     range2.init_sensor(range2_addr);
     
-    for (int count = 4; count < 12; count++) {
-        packet_queue[count] = 1;
-    }
+    optimize_transmissions(packet_queue);
     
     BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
     ble.init(bleInitComplete);
     
-    Ticker ticker;
     ticker.attach(wakeup_event_cb, 0.1 * TIME_SCALE);
     
     while (ble.hasInitialized()) {