workss
Dependencies: mbed BLE_API nRF51822 VL53L0X
main.cpp@34:1d3818f8c1a1, 2019-03-14 (annotated)
- Committer:
- vazbyte
- Date:
- Thu Mar 14 14:19:14 2019 +0000
- Revision:
- 34:1d3818f8c1a1
- Parent:
- 33:3fad9e7238a4
- Child:
- 35:daf134714cee
fixed infinite step bug
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbedAustin | 0:cd5b6733aeb1 | 1 | #include "mbed.h" |
andresag | 19:477567297aac | 2 | #include "ble/BLE.h" |
vazbyte | 24:931eeb8a70fc | 3 | #include "VL53L0X.h" |
vazbyte | 25:0a0805c118c0 | 4 | #include "ble/services/HeartRateService.h" |
vazbyte | 31:d1ceadbc6c44 | 5 | |
vazbyte | 29:6ba8491c1dab | 6 | #define range1_addr (0x56) |
vazbyte | 29:6ba8491c1dab | 7 | #define range2_addr (0x60) |
vazbyte | 29:6ba8491c1dab | 8 | #define range1_XSHUT p15 |
vazbyte | 29:6ba8491c1dab | 9 | #define range2_XSHUT p16 |
vazbyte | 29:6ba8491c1dab | 10 | #define VL53L0_I2C_SDA p30 |
vazbyte | 29:6ba8491c1dab | 11 | #define VL53L0_I2C_SCL p7 |
vazbyte | 31:d1ceadbc6c44 | 12 | #define TIME_SCALE 3 // sensors activated every 100ms * TIME_SCALE |
vazbyte | 29:6ba8491c1dab | 13 | #define DIST_MIN 0 |
vazbyte | 29:6ba8491c1dab | 14 | #define DIST_MAX 22 |
vazbyte | 24:931eeb8a70fc | 15 | |
vazbyte | 24:931eeb8a70fc | 16 | Serial pc(USBTX, USBRX); |
vazbyte | 24:931eeb8a70fc | 17 | static DevI2C devI2c(VL53L0_I2C_SDA, VL53L0_I2C_SCL); |
vazbyte | 24:931eeb8a70fc | 18 | DigitalOut led1(LED1); |
vazbyte | 24:931eeb8a70fc | 19 | DigitalOut led2(LED2); |
vazbyte | 24:931eeb8a70fc | 20 | DigitalOut led(LED3, 1); |
mbedAustin | 9:b33f42191584 | 21 | uint16_t customServiceUUID = 0xA000; |
mbedAustin | 13:62b1d32745ac | 22 | uint16_t readCharUUID = 0xA001; |
mbedAustin | 9:b33f42191584 | 23 | uint16_t writeCharUUID = 0xA002; |
vazbyte | 31:d1ceadbc6c44 | 24 | |
vazbyte | 24:931eeb8a70fc | 25 | static DigitalOut shutdown1_pin(range1_XSHUT); |
vazbyte | 24:931eeb8a70fc | 26 | static VL53L0X range1(&devI2c, &shutdown1_pin, NC); |
vazbyte | 24:931eeb8a70fc | 27 | static DigitalOut shutdown2_pin(range2_XSHUT); |
vazbyte | 24:931eeb8a70fc | 28 | static VL53L0X range2(&devI2c, &shutdown2_pin, NC); |
vazbyte | 31:d1ceadbc6c44 | 29 | |
vazbyte | 33:3fad9e7238a4 | 30 | uint32_t distance1, distance2; |
vazbyte | 33:3fad9e7238a4 | 31 | int dist1, dist2, status1, status2; |
vazbyte | 24:931eeb8a70fc | 32 | |
vazbyte | 31:d1ceadbc6c44 | 33 | const static int cw = 20 / TIME_SCALE; |
vazbyte | 31:d1ceadbc6c44 | 34 | int countdown = cw; |
vazbyte | 31:d1ceadbc6c44 | 35 | bool countdown1_triggered = false, countdown2_triggered = false; |
vazbyte | 31:d1ceadbc6c44 | 36 | bool step_in = false, step_out = false; |
vazbyte | 34:1d3818f8c1a1 | 37 | bool range1_just_triggered = false, range2_just_triggered = false; |
vazbyte | 31:d1ceadbc6c44 | 38 | |
vazbyte | 33:3fad9e7238a4 | 39 | const static char DEVICE_NAME[] = "OCCUPY-CRICHTON-ST"; |
vazbyte | 33:3fad9e7238a4 | 40 | static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE}; |
vazbyte | 31:d1ceadbc6c44 | 41 | |
vazbyte | 25:0a0805c118c0 | 42 | HeartRateService *hrService; |
vazbyte | 26:793d65b08afb | 43 | uint8_t hrmCounter = 0; |
vazbyte | 31:d1ceadbc6c44 | 44 | |
vazbyte | 32:b9306ebceb61 | 45 | void connectionCallback(const Gap::ConnectionCallbackParams_t *) { |
vazbyte | 32:b9306ebceb61 | 46 | printf("Bluetooth connected\n"); |
vazbyte | 32:b9306ebceb61 | 47 | } |
vazbyte | 32:b9306ebceb61 | 48 | |
andresag | 19:477567297aac | 49 | void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *) |
mbedAustin | 1:94152e7d8b5c | 50 | { |
vazbyte | 32:b9306ebceb61 | 51 | printf("Bluetooth disconnected\n"); |
andresag | 22:406127954d1f | 52 | BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising(); |
mbedAustin | 1:94152e7d8b5c | 53 | } |
vazbyte | 31:d1ceadbc6c44 | 54 | |
andresag | 22:406127954d1f | 55 | void bleInitComplete(BLE::InitializationCompleteCallbackContext *params) |
andresag | 22:406127954d1f | 56 | { |
andresag | 22:406127954d1f | 57 | BLE &ble = params->ble; |
andresag | 22:406127954d1f | 58 | ble_error_t error = params->error; |
andresag | 22:406127954d1f | 59 | |
andresag | 22:406127954d1f | 60 | if (error != BLE_ERROR_NONE) { |
andresag | 22:406127954d1f | 61 | return; |
andresag | 22:406127954d1f | 62 | } |
vazbyte | 32:b9306ebceb61 | 63 | |
vazbyte | 32:b9306ebceb61 | 64 | ble.gap().onConnection(connectionCallback); |
andresag | 19:477567297aac | 65 | ble.gap().onDisconnection(disconnectionCallback); |
vazbyte | 25:0a0805c118c0 | 66 | hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER); |
vazbyte | 31:d1ceadbc6c44 | 67 | |
vazbyte | 33:3fad9e7238a4 | 68 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); |
vazbyte | 33:3fad9e7238a4 | 69 | ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
vazbyte | 33:3fad9e7238a4 | 70 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); |
vazbyte | 33:3fad9e7238a4 | 71 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); |
vazbyte | 33:3fad9e7238a4 | 72 | ble.gap().setAdvertisingInterval(100); |
vazbyte | 25:0a0805c118c0 | 73 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR); |
vazbyte | 31:d1ceadbc6c44 | 74 | |
andresag | 19:477567297aac | 75 | ble.gap().startAdvertising(); |
andresag | 22:406127954d1f | 76 | } |
vazbyte | 31:d1ceadbc6c44 | 77 | |
vazbyte | 27:903ec28ea7a0 | 78 | int format_dist(int distance) { |
vazbyte | 27:903ec28ea7a0 | 79 | int result; |
vazbyte | 27:903ec28ea7a0 | 80 | |
vazbyte | 27:903ec28ea7a0 | 81 | if (distance > 1270) |
vazbyte | 27:903ec28ea7a0 | 82 | result = 127; |
vazbyte | 27:903ec28ea7a0 | 83 | else |
vazbyte | 27:903ec28ea7a0 | 84 | result = distance/10; |
vazbyte | 27:903ec28ea7a0 | 85 | |
vazbyte | 27:903ec28ea7a0 | 86 | return result; |
vazbyte | 27:903ec28ea7a0 | 87 | } |
vazbyte | 31:d1ceadbc6c44 | 88 | |
vazbyte | 23:52e8e05df60c | 89 | void wakeup_event_cb() { |
vazbyte | 23:52e8e05df60c | 90 | led != led; |
vazbyte | 24:931eeb8a70fc | 91 | |
vazbyte | 29:6ba8491c1dab | 92 | if (countdown1_triggered) { |
vazbyte | 29:6ba8491c1dab | 93 | countdown--; |
vazbyte | 29:6ba8491c1dab | 94 | if (countdown == 0) { |
vazbyte | 29:6ba8491c1dab | 95 | countdown1_triggered = false; |
vazbyte | 29:6ba8491c1dab | 96 | } |
vazbyte | 29:6ba8491c1dab | 97 | } |
vazbyte | 29:6ba8491c1dab | 98 | if (countdown2_triggered) { |
vazbyte | 29:6ba8491c1dab | 99 | countdown--; |
vazbyte | 29:6ba8491c1dab | 100 | if (countdown == 0) { |
vazbyte | 29:6ba8491c1dab | 101 | countdown2_triggered = false; |
vazbyte | 29:6ba8491c1dab | 102 | } |
vazbyte | 29:6ba8491c1dab | 103 | } |
vazbyte | 31:d1ceadbc6c44 | 104 | |
vazbyte | 24:931eeb8a70fc | 105 | status1 = range1.get_distance(&distance1); |
vazbyte | 24:931eeb8a70fc | 106 | status2 = range2.get_distance(&distance2); |
vazbyte | 26:793d65b08afb | 107 | |
vazbyte | 27:903ec28ea7a0 | 108 | dist1 = format_dist(distance1); |
vazbyte | 27:903ec28ea7a0 | 109 | dist2 = format_dist(distance2); |
vazbyte | 31:d1ceadbc6c44 | 110 | |
vazbyte | 24:931eeb8a70fc | 111 | if (status1 == VL53L0X_ERROR_NONE) { |
vazbyte | 31:d1ceadbc6c44 | 112 | printf("Range1 [mm]: %6ld\r\n", dist1); |
vazbyte | 29:6ba8491c1dab | 113 | if (dist1 > DIST_MIN && dist1 < DIST_MAX) { |
vazbyte | 24:931eeb8a70fc | 114 | led1 = 0; |
vazbyte | 29:6ba8491c1dab | 115 | |
vazbyte | 29:6ba8491c1dab | 116 | if (!countdown1_triggered && !countdown2_triggered) { |
vazbyte | 29:6ba8491c1dab | 117 | countdown1_triggered = true; |
vazbyte | 29:6ba8491c1dab | 118 | countdown = cw; |
vazbyte | 34:1d3818f8c1a1 | 119 | } else if (countdown2_triggered && !range1_just_triggered) { |
vazbyte | 29:6ba8491c1dab | 120 | printf("STEP IN DETECTED\n"); |
vazbyte | 32:b9306ebceb61 | 121 | hrmCounter = 1; |
vazbyte | 32:b9306ebceb61 | 122 | hrService->updateHeartRate(hrmCounter); |
vazbyte | 29:6ba8491c1dab | 123 | countdown2_triggered = false; |
vazbyte | 29:6ba8491c1dab | 124 | } |
vazbyte | 34:1d3818f8c1a1 | 125 | |
vazbyte | 34:1d3818f8c1a1 | 126 | range1_just_triggered = true; |
vazbyte | 24:931eeb8a70fc | 127 | } |
vazbyte | 24:931eeb8a70fc | 128 | else { |
vazbyte | 24:931eeb8a70fc | 129 | led1 = 1; |
vazbyte | 34:1d3818f8c1a1 | 130 | range1_just_triggered = false; |
vazbyte | 24:931eeb8a70fc | 131 | } |
vazbyte | 24:931eeb8a70fc | 132 | } else { |
vazbyte | 34:1d3818f8c1a1 | 133 | // printf("Range1 [mm]: --\r\n"); |
vazbyte | 24:931eeb8a70fc | 134 | led1 = 1; |
vazbyte | 34:1d3818f8c1a1 | 135 | range2_just_triggered = false; |
vazbyte | 24:931eeb8a70fc | 136 | } |
vazbyte | 24:931eeb8a70fc | 137 | if (status2 == VL53L0X_ERROR_NONE) { |
vazbyte | 31:d1ceadbc6c44 | 138 | printf("Range2 [mm]: %6ld\r\n", dist2); |
vazbyte | 29:6ba8491c1dab | 139 | if (dist2 > DIST_MIN && dist2 < DIST_MAX) { |
vazbyte | 27:903ec28ea7a0 | 140 | led2 = 0; |
vazbyte | 29:6ba8491c1dab | 141 | |
vazbyte | 29:6ba8491c1dab | 142 | if (!countdown1_triggered && !countdown2_triggered) { |
vazbyte | 29:6ba8491c1dab | 143 | countdown2_triggered = true; |
vazbyte | 29:6ba8491c1dab | 144 | countdown = cw; |
vazbyte | 34:1d3818f8c1a1 | 145 | } else if (countdown1_triggered && !range2_just_triggered) { |
vazbyte | 29:6ba8491c1dab | 146 | printf("STEP OUT DETECTED\n"); |
vazbyte | 32:b9306ebceb61 | 147 | hrmCounter = 16; |
vazbyte | 32:b9306ebceb61 | 148 | hrService->updateHeartRate(hrmCounter); |
vazbyte | 29:6ba8491c1dab | 149 | countdown1_triggered = false; |
vazbyte | 29:6ba8491c1dab | 150 | } |
vazbyte | 34:1d3818f8c1a1 | 151 | |
vazbyte | 34:1d3818f8c1a1 | 152 | range2_just_triggered = true; |
vazbyte | 34:1d3818f8c1a1 | 153 | } |
vazbyte | 34:1d3818f8c1a1 | 154 | else { |
vazbyte | 34:1d3818f8c1a1 | 155 | led2 = 1; |
vazbyte | 34:1d3818f8c1a1 | 156 | range2_just_triggered = false; |
vazbyte | 27:903ec28ea7a0 | 157 | } |
vazbyte | 24:931eeb8a70fc | 158 | } else { |
vazbyte | 34:1d3818f8c1a1 | 159 | // printf("Range2 [mm]: --\r\n"); |
vazbyte | 24:931eeb8a70fc | 160 | led2 = 1; |
vazbyte | 34:1d3818f8c1a1 | 161 | range2_just_triggered = false; |
vazbyte | 24:931eeb8a70fc | 162 | } |
vazbyte | 23:52e8e05df60c | 163 | } |
vazbyte | 31:d1ceadbc6c44 | 164 | |
andresag | 22:406127954d1f | 165 | int main(void) |
andresag | 22:406127954d1f | 166 | { |
vazbyte | 24:931eeb8a70fc | 167 | range1.init_sensor(range1_addr); |
vazbyte | 24:931eeb8a70fc | 168 | range2.init_sensor(range2_addr); |
vazbyte | 24:931eeb8a70fc | 169 | |
andresag | 22:406127954d1f | 170 | BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE); |
andresag | 22:406127954d1f | 171 | ble.init(bleInitComplete); |
andresag | 22:406127954d1f | 172 | |
vazbyte | 23:52e8e05df60c | 173 | Ticker ticker; |
vazbyte | 31:d1ceadbc6c44 | 174 | ticker.attach(wakeup_event_cb, 0.1 * TIME_SCALE); |
vazbyte | 24:931eeb8a70fc | 175 | |
vazbyte | 23:52e8e05df60c | 176 | while (ble.hasInitialized()) { |
vazbyte | 24:931eeb8a70fc | 177 | ble.waitForEvent(); |
mbedAustin | 2:e84c13abc479 | 178 | } |
andresag | 20:fcc752d401ec | 179 | } |