Diff: source/main.cpp

Revision:

19:1998218bbb63

Parent:

18:4cbd88c0a374

Child:

21:36bd27c83b35

--- a/source/main.cpp	Wed Nov 21 10:40:41 2018 +0000
+++ b/source/main.cpp	Tue Nov 27 17:01:37 2018 +0000
@@ -1,19 +1,3 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2006-2014 ARM Limited
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
 #include <events/mbed_events.h>
 #include <math.h> 
 
@@ -24,176 +8,174 @@
 #include "ble/Gap.h"
 #include "ble/services/HeartRateService.h"
 
-DigitalOut Led0(p19);
-DigitalOut Led1(p20);
-DigitalOut Led2(p11);
-DigitalOut Led3(p12);
-DigitalOut Led4(p13);
-DigitalOut Led5(p14);
-DigitalOut Led6(p15);
-DigitalOut Led7(p16);
-DigitalOut Led8(p17);
-DigitalOut Led9(p18);
+DigitalOut Led0(p3);
+DigitalOut Led1(p4);
+DigitalOut Led2(p28);
+DigitalOut Led3(p29);
+DigitalOut Led4(p30);
+DigitalOut Led5(p31);
+DigitalOut Led6(p11);
+DigitalOut Led7(p12);
+DigitalOut Led8(p13);
+DigitalOut Led9(p14);
+DigitalOut Led10(p15);
+DigitalOut Led11(p16);
+DigitalOut Led12(p17);
+DigitalOut Led13(p18);
+DigitalOut Led14(p19);
+DigitalOut Led15(p20);
+DigitalOut Led16(p22);
+DigitalOut Led17(p23);
+DigitalOut Led18(p24);
+DigitalOut Led19(p25);
 
-//DigitalOut led1(LED1, 1);
+DigitalIn pb(p8);
 
+int USV_count = 1;
+int old_pb = 1;
+int new_pb;
+    
 const static char     DEVICE_NAME[] = "ProVaida";
 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
+short hrmCounter = 0;
+static HeartRateService* hrService;
+
+bool BLE_conn = false;
 
 int16_t destination[3];
-short hrmCounter = 0;
 double step_threshold = 14.45;
 
 double oldAcceleration = 0.0;
 int callback_cycles = 1; // used to be 4
 int step;
+int totalsteps = 0;
+
 int run_threshold = 5; // used to be 7
 int run_count = 0;
 
-int totalsteps = 0;
-
-static HeartRateService* hrService;
 MPU9250 mpu = MPU9250(P0_26, P0_27);
 
-static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);
+static EventQueue eventQueue(16 * EVENTS_EVENT_SIZE);
 
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
 {
     BLE::Instance().gap().startAdvertising();
 }
 
-void updateSensorValue() {
-    step = 0;
-    callback_cycles++;
+void updateSensorValue() {    
+    wait(.001);
+    new_pb = pb;
+    if ((new_pb == 0) && (old_pb == 1)) {
+        USV_count++;
+        printf("count: %i\n", USV_count);
+    }
+    if (USV_count % 2) {        
+        step = 0;
+        callback_cycles++;
     
-    mpu.readAccelData(destination);
+        mpu.readAccelData(destination);
     
-    double acc_x = destination[0] / 1000.0;
-    double acc_y = destination[1] / 1000.0;
-    double acc_z = destination[2] / 1000.0;
+        double acc_x = destination[0] / 1000.0;
+        double acc_y = destination[1] / 1000.0;
+        double acc_z = destination[2] / 1000.0;
     
-    double sqr_acc_x = acc_x*acc_x;
-    double sqr_acc_y = acc_y*acc_y;
-    double sqr_acc_z = acc_z*acc_z;
+        double sqr_acc_x = acc_x*acc_x;
+        double sqr_acc_y = acc_y*acc_y;
+        double sqr_acc_z = acc_z*acc_z;
+    
+        double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
+        double accel = sqrt(sum_acc);
     
-    double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
-    double accel = sqrt(sum_acc);
-
-    //printf("calback cycles: " );
-    //printf("%i\n", callback_cycles);  
-     
-    if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 3)) {
-        if (callback_cycles <= run_threshold) {
-            if (run_count >= 2) {
-                step = 2;
+        if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 3)) {
+            if (callback_cycles <= run_threshold) {
+                if (run_count >= 2) {
+                    step = 2;
+                }
+                else {
+                    step = 1;
+                    run_count++;
+                }
             }
             else {
                 step = 1;
-                run_count++;
+                run_count = 0;    
             }
-        }
-        else {
-            step = 1;
-            run_count = 0;    
+            callback_cycles = 0;
         }
-        callback_cycles = 0;
-    }
-    if (step != 0)
-    {
-        printf("STEP: " );
-        printf("%i\n", step);          
+        if (step != 0)
+        {
+            totalsteps++;                
+            Led0 = ((totalsteps >> 0) % 2);
+            Led1 = ((totalsteps >> 1) % 2);
+            Led2 = ((totalsteps >> 2) % 2);
+            Led3 = ((totalsteps >> 3) % 2);
+            Led4 = ((totalsteps >> 4) % 2);
+            Led5 = ((totalsteps >> 5) % 2);
+            Led6 = ((totalsteps >> 6) % 2);
+            Led7 = ((totalsteps >> 7) % 2);
+            Led8 = ((totalsteps >> 8) % 2);
+            Led9 = ((totalsteps >> 9) % 2);
+            Led10 = ((totalsteps >> 10) % 2);
+            Led11 = ((totalsteps >> 11) % 2);
+            Led12 = ((totalsteps >> 12) % 2);
+            Led13 = ((totalsteps >> 12) % 2);
+            Led14 = ((totalsteps >> 14) % 2);
+            Led15 = ((totalsteps >> 15) % 2);
+            Led16 = ((totalsteps >> 16) % 2);
+            Led17 = ((totalsteps >> 17) % 2);
+            Led18 = ((totalsteps >> 18) % 2);
+            Led19 = ((totalsteps >> 19) % 2);
+            if ((totalsteps >> 20) % 2 == 1) {
+                totalsteps = 0;
+            }   
+        }
+        oldAcceleration = accel;
 
-        totalsteps++;        
-        
-        printf("totalSTEPs: " );
-        printf("%i\n", totalsteps);  
-        
-        Led0 = ((totalsteps >> 0) % 2);
-        Led1 = ((totalsteps >> 1) % 2);
-        Led2 = ((totalsteps >> 2) % 2);
-        Led3 = ((totalsteps >> 3) % 2);
-        Led4 = ((totalsteps >> 4) % 2);
-        Led5 = ((totalsteps >> 5) % 2);
-        Led6 = ((totalsteps >> 6) % 2);
-        Led7 = ((totalsteps >> 7) % 2);
-        Led8 = ((totalsteps >> 8) % 2);
-        Led9 = ((totalsteps >> 9) % 2);
+    } else if (totalsteps != 0) {
+       totalsteps = 0;
+    }         
+    old_pb = new_pb;    
+    
+    if (BLE_conn) {
+        hrmCounter = (short) step;    
+        hrService->updateHeartRate(hrmCounter);
     }
-    
-    oldAcceleration = accel;
-    
-    hrmCounter = (short) step;
-    //printf("STEP: " );
-    //printf("%hu\n", hrmCounter);  
-    
-    hrService->updateHeartRate(hrmCounter);
 }
 
 void blinkCallback(void)
 {
-    //led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
     BLE &ble = BLE::Instance();
     if (ble.gap().getState().connected) {
         eventQueue.call(updateSensorValue);
     }
 }
 
-/**
- * This function is called when the ble initialization process has failed
- */
-void onBleInitError(BLE &ble, ble_error_t error)
-{
-    /* Initialization error handling should go here */
-}
+void onBleInitError(BLE &ble, ble_error_t error) {}
 
-void printMacAddress()
-{
-    /* Print out device MAC address to the console*/
-    Gap::AddressType_t addr_type;
-    Gap::Address_t address;
-    BLE::Instance().gap().getAddress(&addr_type, address);
-    printf("DEVICE MAC ADDRESS: ");
-    for (int i = 5; i >= 1; i--){
-        printf("%02x:", address[i]);
-    }
-    printf("%02x\r\n", address[0]);
-}
-
-/**
- * Callback triggered when the ble initialization process has finished
- */
 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
 {
     BLE&        ble   = params->ble;
     ble_error_t error = params->error;
 
-    if (error != BLE_ERROR_NONE) {
-        /* In case of error, forward the error handling to onBleInitError */
+    if (error != BLE_ERROR_NONE) {        
         onBleInitError(ble, error);
         return;
     }
 
-    /* Ensure that it is the default instance of BLE */
     if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
         return;
     }
 
     ble.gap().onDisconnection(disconnectionCallback);
 
-    /* Setup primary service */
     hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
 
-    /* Setup advertising */
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
-    ble.gap().setAdvertisingInterval(1000); /* 1000ms */
+    ble.gap().setAdvertisingInterval(1000); 
     ble.gap().startAdvertising();
-
-
-    printMacAddress();
-
 }
 
 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
@@ -202,16 +184,27 @@
 }
 
 int main()
-{    
+{   
+    
+    pb.mode(PullUp);
     mpu.initMPU9250();
     
-    eventQueue.call_every(80, blinkCallback);
+    while (1) {
+        updateSensorValue();
+        
+        BLE &ble = BLE::Instance();
+        ble.onEventsToProcess(scheduleBleEventsProcessing);
+        ble.init(bleInitComplete);
+        
+        if (ble.gap().getState().connected) {
+            BLE_conn = true;
+            //eventQueue.call(updateSensorValue);
+        }        
 
-    BLE &ble = BLE::Instance();
-    ble.onEventsToProcess(scheduleBleEventsProcessing);
-    ble.init(bleInitComplete);
-
-    eventQueue.dispatch_forever();
+        //eventQueue.dispatch_forever();
+        
+        wait(0.1);
+    }
 
     return 0;
 }