workss
Dependencies: mbed BLE_API nRF51822 VL53L0X
main.cpp@39:067ba1a475a5, 2019-03-17 (annotated)
- Committer:
- vazbyte
- Date:
- Sun Mar 17 15:42:00 2019 +0000
- Revision:
- 39:067ba1a475a5
- Parent:
- 38:7035f67d8105
- Child:
- 40:d332edae807d
bugs fixed: queue working with BLE
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vazbyte | 35:daf134714cee | 1 | #include <stdio.h> |
vazbyte | 35:daf134714cee | 2 | #include <math.h> |
mbedAustin | 0:cd5b6733aeb1 | 3 | #include "mbed.h" |
andresag | 19:477567297aac | 4 | #include "ble/BLE.h" |
vazbyte | 24:931eeb8a70fc | 5 | #include "VL53L0X.h" |
vazbyte | 25:0a0805c118c0 | 6 | #include "ble/services/HeartRateService.h" |
vazbyte | 31:d1ceadbc6c44 | 7 | |
vazbyte | 29:6ba8491c1dab | 8 | #define range1_addr (0x56) |
vazbyte | 29:6ba8491c1dab | 9 | #define range2_addr (0x60) |
vazbyte | 29:6ba8491c1dab | 10 | #define range1_XSHUT p15 |
vazbyte | 29:6ba8491c1dab | 11 | #define range2_XSHUT p16 |
vazbyte | 29:6ba8491c1dab | 12 | #define VL53L0_I2C_SDA p30 |
vazbyte | 29:6ba8491c1dab | 13 | #define VL53L0_I2C_SCL p7 |
vazbyte | 35:daf134714cee | 14 | #define TIME_SCALE 3 // sensors activated every 100ms * TIME_SCALE = 0.3 seconds |
vazbyte | 29:6ba8491c1dab | 15 | #define DIST_MIN 0 |
vazbyte | 29:6ba8491c1dab | 16 | #define DIST_MAX 22 |
vazbyte | 35:daf134714cee | 17 | //#define TIMESTAMP_FREQ 3000 // PROD: time granularity is 100ms * TIMESTAMP_FREQ = 5 minutes |
vazbyte | 35:daf134714cee | 18 | #define TIMESTAMP_FREQ 300 // DEV: time granularity is 100ms * TIMESTAMP_FREQ = 30 seconds |
vazbyte | 35:daf134714cee | 19 | //#define MAX_TIME 1728000 // PROD: keep track of 100ms * MAX_TIME = 2880 minutes = 2 days |
vazbyte | 35:daf134714cee | 20 | #define MAX_TIME 172800 // DEV: keep track of 100ms * MAX_TIME = 288 minutes = 4.8 hours |
vazbyte | 24:931eeb8a70fc | 21 | |
vazbyte | 24:931eeb8a70fc | 22 | Serial pc(USBTX, USBRX); |
vazbyte | 24:931eeb8a70fc | 23 | static DevI2C devI2c(VL53L0_I2C_SDA, VL53L0_I2C_SCL); |
vazbyte | 24:931eeb8a70fc | 24 | DigitalOut led1(LED1); |
vazbyte | 24:931eeb8a70fc | 25 | DigitalOut led2(LED2); |
vazbyte | 24:931eeb8a70fc | 26 | DigitalOut led(LED3, 1); |
mbedAustin | 9:b33f42191584 | 27 | uint16_t customServiceUUID = 0xA000; |
mbedAustin | 13:62b1d32745ac | 28 | uint16_t readCharUUID = 0xA001; |
mbedAustin | 9:b33f42191584 | 29 | uint16_t writeCharUUID = 0xA002; |
vazbyte | 31:d1ceadbc6c44 | 30 | |
vazbyte | 24:931eeb8a70fc | 31 | static DigitalOut shutdown1_pin(range1_XSHUT); |
vazbyte | 24:931eeb8a70fc | 32 | static VL53L0X range1(&devI2c, &shutdown1_pin, NC); |
vazbyte | 24:931eeb8a70fc | 33 | static DigitalOut shutdown2_pin(range2_XSHUT); |
vazbyte | 24:931eeb8a70fc | 34 | static VL53L0X range2(&devI2c, &shutdown2_pin, NC); |
vazbyte | 31:d1ceadbc6c44 | 35 | |
vazbyte | 33:3fad9e7238a4 | 36 | uint32_t distance1, distance2; |
vazbyte | 33:3fad9e7238a4 | 37 | int dist1, dist2, status1, status2; |
vazbyte | 38:7035f67d8105 | 38 | int packet_queue[4000] = {0}; |
vazbyte | 38:7035f67d8105 | 39 | int packet_queue_index = 0; |
vazbyte | 24:931eeb8a70fc | 40 | |
vazbyte | 31:d1ceadbc6c44 | 41 | const static int cw = 20 / TIME_SCALE; |
vazbyte | 35:daf134714cee | 42 | const static int time_cycle = TIMESTAMP_FREQ / TIME_SCALE; |
vazbyte | 35:daf134714cee | 43 | const static int maximum_time = MAX_TIME / TIMESTAMP_FREQ; |
vazbyte | 31:d1ceadbc6c44 | 44 | int countdown = cw; |
vazbyte | 31:d1ceadbc6c44 | 45 | bool countdown1_triggered = false, countdown2_triggered = false; |
vazbyte | 31:d1ceadbc6c44 | 46 | bool step_in = false, step_out = false; |
vazbyte | 34:1d3818f8c1a1 | 47 | bool range1_just_triggered = false, range2_just_triggered = false; |
vazbyte | 38:7035f67d8105 | 48 | bool master_connected = false; |
vazbyte | 31:d1ceadbc6c44 | 49 | |
vazbyte | 33:3fad9e7238a4 | 50 | const static char DEVICE_NAME[] = "OCCUPY-CRICHTON-ST"; |
vazbyte | 33:3fad9e7238a4 | 51 | static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE}; |
vazbyte | 35:daf134714cee | 52 | int * encoded_array; |
vazbyte | 38:7035f67d8105 | 53 | int timestamp = time_cycle, current_time = 0; |
vazbyte | 31:d1ceadbc6c44 | 54 | |
vazbyte | 25:0a0805c118c0 | 55 | HeartRateService *hrService; |
vazbyte | 26:793d65b08afb | 56 | uint8_t hrmCounter = 0; |
vazbyte | 27:903ec28ea7a0 | 57 | int format_dist(int distance) { |
vazbyte | 27:903ec28ea7a0 | 58 | int result; |
vazbyte | 36:fb7f31e53ed9 | 59 | if (distance > 2550) |
vazbyte | 36:fb7f31e53ed9 | 60 | result = 255; |
vazbyte | 27:903ec28ea7a0 | 61 | else |
vazbyte | 27:903ec28ea7a0 | 62 | result = distance/10; |
vazbyte | 27:903ec28ea7a0 | 63 | return result; |
vazbyte | 27:903ec28ea7a0 | 64 | } |
vazbyte | 35:daf134714cee | 65 | |
vazbyte | 35:daf134714cee | 66 | void dec_to_bin(int decimal, int bin[]) { |
vazbyte | 35:daf134714cee | 67 | int temp; |
vazbyte | 35:daf134714cee | 68 | // encoding little-endian |
vazbyte | 35:daf134714cee | 69 | for (int i = 10; i >= 0; i--) { |
vazbyte | 35:daf134714cee | 70 | temp = decimal >> i; |
vazbyte | 35:daf134714cee | 71 | bin[i] = temp&1; |
vazbyte | 35:daf134714cee | 72 | } |
vazbyte | 35:daf134714cee | 73 | } |
vazbyte | 35:daf134714cee | 74 | |
vazbyte | 35:daf134714cee | 75 | int bin_to_dec(int binary) { |
vazbyte | 35:daf134714cee | 76 | int rem, temp, dec = 0, b = 1; |
vazbyte | 35:daf134714cee | 77 | temp = binary; |
vazbyte | 35:daf134714cee | 78 | while (temp > 0) |
vazbyte | 35:daf134714cee | 79 | { |
vazbyte | 35:daf134714cee | 80 | rem = temp % 10; |
vazbyte | 35:daf134714cee | 81 | dec = dec + rem * b; |
vazbyte | 35:daf134714cee | 82 | b *= 2; |
vazbyte | 35:daf134714cee | 83 | temp /= 10; |
vazbyte | 35:daf134714cee | 84 | } |
vazbyte | 35:daf134714cee | 85 | return dec; |
vazbyte | 35:daf134714cee | 86 | } |
vazbyte | 35:daf134714cee | 87 | |
vazbyte | 35:daf134714cee | 88 | void encode_bin(int direction, int time, int encoding[]) { |
vazbyte | 35:daf134714cee | 89 | time = time % maximum_time; |
vazbyte | 38:7035f67d8105 | 90 | int bin_timestamp[11] = {0}; |
vazbyte | 38:7035f67d8105 | 91 | dec_to_bin(time, bin_timestamp); |
vazbyte | 35:daf134714cee | 92 | |
vazbyte | 35:daf134714cee | 93 | // for checking transmission errors: |
vazbyte | 37:5792986fea5d | 94 | // sending as 2 messages; designating them with 0 (1st part) and 1 (2nd part) |
vazbyte | 35:daf134714cee | 95 | encoding [8] = 1; |
vazbyte | 35:daf134714cee | 96 | encoding [0] = 0; |
vazbyte | 35:daf134714cee | 97 | |
vazbyte | 35:daf134714cee | 98 | // used to send the current time when Bluetooth connection established |
vazbyte | 35:daf134714cee | 99 | if (direction == -1) { |
vazbyte | 35:daf134714cee | 100 | encoding[15] = 1; |
vazbyte | 35:daf134714cee | 101 | encoding[7] = 1; |
vazbyte | 35:daf134714cee | 102 | |
vazbyte | 35:daf134714cee | 103 | } else { |
vazbyte | 35:daf134714cee | 104 | encoding[15] = 0; |
vazbyte | 35:daf134714cee | 105 | encoding[7] = 0; |
vazbyte | 35:daf134714cee | 106 | |
vazbyte | 35:daf134714cee | 107 | // to distinguish whether movement was IN (1) or OUT (0) |
vazbyte | 35:daf134714cee | 108 | if (direction == 1) { |
vazbyte | 35:daf134714cee | 109 | encoding[1] = 1; |
vazbyte | 35:daf134714cee | 110 | } else { |
vazbyte | 35:daf134714cee | 111 | encoding[1] = 0; |
vazbyte | 35:daf134714cee | 112 | } |
vazbyte | 35:daf134714cee | 113 | } |
vazbyte | 35:daf134714cee | 114 | |
vazbyte | 38:7035f67d8105 | 115 | if (!master_connected) { |
vazbyte | 38:7035f67d8105 | 116 | int count = 10; |
vazbyte | 38:7035f67d8105 | 117 | for (int i = 14; i >= 2; i--) { |
vazbyte | 38:7035f67d8105 | 118 | if (i == 8 || i == 7) { } |
vazbyte | 38:7035f67d8105 | 119 | else { |
vazbyte | 38:7035f67d8105 | 120 | encoding[i] = bin_timestamp[count]; |
vazbyte | 38:7035f67d8105 | 121 | count--; |
vazbyte | 38:7035f67d8105 | 122 | } |
vazbyte | 35:daf134714cee | 123 | } |
vazbyte | 35:daf134714cee | 124 | } |
vazbyte | 35:daf134714cee | 125 | } |
vazbyte | 35:daf134714cee | 126 | |
vazbyte | 35:daf134714cee | 127 | void create_packets(int encoding[], double& packet1, double& packet2) { |
vazbyte | 35:daf134714cee | 128 | double binary1 = 0, binary2 = 0; |
vazbyte | 35:daf134714cee | 129 | for (int i = 0; i < 8; i++) { |
vazbyte | 35:daf134714cee | 130 | binary1 += encoding[i] * pow(10, (double)i); |
vazbyte | 35:daf134714cee | 131 | binary2 += encoding[8+i] * pow(10, (double)i); |
vazbyte | 35:daf134714cee | 132 | } |
vazbyte | 35:daf134714cee | 133 | packet1 = bin_to_dec(binary1); |
vazbyte | 35:daf134714cee | 134 | packet2 = bin_to_dec(binary2); |
vazbyte | 35:daf134714cee | 135 | } |
vazbyte | 36:fb7f31e53ed9 | 136 | |
vazbyte | 36:fb7f31e53ed9 | 137 | void send_packet(int direction, int time) { |
vazbyte | 36:fb7f31e53ed9 | 138 | int encoding [16] = {0}; |
vazbyte | 36:fb7f31e53ed9 | 139 | encode_bin(direction, time, encoding); |
vazbyte | 36:fb7f31e53ed9 | 140 | double packet1, packet2; |
vazbyte | 36:fb7f31e53ed9 | 141 | create_packets(encoding, packet1, packet2); |
vazbyte | 36:fb7f31e53ed9 | 142 | int p1 = (int)packet1; |
vazbyte | 36:fb7f31e53ed9 | 143 | int p2 = (int)packet2; |
vazbyte | 36:fb7f31e53ed9 | 144 | |
vazbyte | 36:fb7f31e53ed9 | 145 | hrmCounter = p1; |
vazbyte | 36:fb7f31e53ed9 | 146 | hrService->updateHeartRate(hrmCounter); |
vazbyte | 36:fb7f31e53ed9 | 147 | printf("sent packet %i\n", p1); |
vazbyte | 36:fb7f31e53ed9 | 148 | hrmCounter = p2; |
vazbyte | 36:fb7f31e53ed9 | 149 | hrService->updateHeartRate(hrmCounter); |
vazbyte | 36:fb7f31e53ed9 | 150 | printf("sent packet %i\n", p2); |
vazbyte | 36:fb7f31e53ed9 | 151 | } |
vazbyte | 36:fb7f31e53ed9 | 152 | |
vazbyte | 38:7035f67d8105 | 153 | void send_packet_from_queue(int current_index) { |
vazbyte | 38:7035f67d8105 | 154 | double packet1 = packet_queue[current_index]; |
vazbyte | 38:7035f67d8105 | 155 | double packet2 = packet_queue[current_index + 1]; |
vazbyte | 38:7035f67d8105 | 156 | |
vazbyte | 38:7035f67d8105 | 157 | int p1 = (int)packet1; |
vazbyte | 38:7035f67d8105 | 158 | int p2 = (int)packet2; |
vazbyte | 38:7035f67d8105 | 159 | |
vazbyte | 38:7035f67d8105 | 160 | hrmCounter = p1; |
vazbyte | 38:7035f67d8105 | 161 | hrService->updateHeartRate(hrmCounter); |
vazbyte | 38:7035f67d8105 | 162 | printf("sent packet %i\n", p1); |
vazbyte | 38:7035f67d8105 | 163 | hrmCounter = p2; |
vazbyte | 38:7035f67d8105 | 164 | hrService->updateHeartRate(hrmCounter); |
vazbyte | 38:7035f67d8105 | 165 | printf("sent packet %i\n", p2); |
vazbyte | 38:7035f67d8105 | 166 | } |
vazbyte | 38:7035f67d8105 | 167 | |
vazbyte | 38:7035f67d8105 | 168 | void enqueue_packet(int direction, int time, int queue[]) { |
vazbyte | 38:7035f67d8105 | 169 | int encoding [16] = {0}; |
vazbyte | 38:7035f67d8105 | 170 | encode_bin(direction, time, encoding); |
vazbyte | 38:7035f67d8105 | 171 | double packet1, packet2; |
vazbyte | 38:7035f67d8105 | 172 | create_packets(encoding, packet1, packet2); |
vazbyte | 38:7035f67d8105 | 173 | int p1 = (int)packet1; |
vazbyte | 38:7035f67d8105 | 174 | int p2 = (int)packet2; |
vazbyte | 38:7035f67d8105 | 175 | |
vazbyte | 38:7035f67d8105 | 176 | packet_queue[packet_queue_index] = p1; |
vazbyte | 38:7035f67d8105 | 177 | printf("enqueued packet %i\n", p1); |
vazbyte | 38:7035f67d8105 | 178 | packet_queue[packet_queue_index + 1] = p2; |
vazbyte | 38:7035f67d8105 | 179 | printf("enqueued packet %i\n", p2); |
vazbyte | 38:7035f67d8105 | 180 | |
vazbyte | 38:7035f67d8105 | 181 | ::packet_queue_index += 2; |
vazbyte | 38:7035f67d8105 | 182 | } |
vazbyte | 38:7035f67d8105 | 183 | |
vazbyte | 36:fb7f31e53ed9 | 184 | void connectionCallback(const Gap::ConnectionCallbackParams_t *) { |
vazbyte | 36:fb7f31e53ed9 | 185 | printf("Bluetooth connected at %i\n", current_time); |
vazbyte | 38:7035f67d8105 | 186 | ::master_connected = true; |
vazbyte | 36:fb7f31e53ed9 | 187 | send_packet(-1, current_time); |
vazbyte | 38:7035f67d8105 | 188 | |
vazbyte | 38:7035f67d8105 | 189 | int current_index = 0; |
vazbyte | 38:7035f67d8105 | 190 | while (current_index < packet_queue_index) { |
vazbyte | 38:7035f67d8105 | 191 | send_packet_from_queue(current_index); |
vazbyte | 38:7035f67d8105 | 192 | current_index += 2; |
vazbyte | 38:7035f67d8105 | 193 | } |
vazbyte | 38:7035f67d8105 | 194 | ::packet_queue_index = 0; |
vazbyte | 36:fb7f31e53ed9 | 195 | } |
vazbyte | 36:fb7f31e53ed9 | 196 | |
vazbyte | 36:fb7f31e53ed9 | 197 | void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *) |
vazbyte | 36:fb7f31e53ed9 | 198 | { |
vazbyte | 36:fb7f31e53ed9 | 199 | printf("Bluetooth disconnected at %i\n", current_time); |
vazbyte | 38:7035f67d8105 | 200 | ::master_connected = false; |
vazbyte | 39:067ba1a475a5 | 201 | ::timestamp = time_cycle; |
vazbyte | 36:fb7f31e53ed9 | 202 | BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising(); |
vazbyte | 36:fb7f31e53ed9 | 203 | } |
vazbyte | 36:fb7f31e53ed9 | 204 | |
vazbyte | 36:fb7f31e53ed9 | 205 | void bleInitComplete(BLE::InitializationCompleteCallbackContext *params) |
vazbyte | 36:fb7f31e53ed9 | 206 | { |
vazbyte | 36:fb7f31e53ed9 | 207 | BLE &ble = params->ble; |
vazbyte | 36:fb7f31e53ed9 | 208 | ble_error_t error = params->error; |
vazbyte | 36:fb7f31e53ed9 | 209 | |
vazbyte | 36:fb7f31e53ed9 | 210 | if (error != BLE_ERROR_NONE) { |
vazbyte | 36:fb7f31e53ed9 | 211 | return; |
vazbyte | 36:fb7f31e53ed9 | 212 | } |
vazbyte | 36:fb7f31e53ed9 | 213 | |
vazbyte | 36:fb7f31e53ed9 | 214 | ble.gap().onConnection(connectionCallback); |
vazbyte | 36:fb7f31e53ed9 | 215 | ble.gap().onDisconnection(disconnectionCallback); |
vazbyte | 36:fb7f31e53ed9 | 216 | hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER); |
vazbyte | 36:fb7f31e53ed9 | 217 | |
vazbyte | 36:fb7f31e53ed9 | 218 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); |
vazbyte | 36:fb7f31e53ed9 | 219 | ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
vazbyte | 36:fb7f31e53ed9 | 220 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); |
vazbyte | 36:fb7f31e53ed9 | 221 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); |
vazbyte | 36:fb7f31e53ed9 | 222 | ble.gap().setAdvertisingInterval(100); |
vazbyte | 36:fb7f31e53ed9 | 223 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR); |
vazbyte | 36:fb7f31e53ed9 | 224 | |
vazbyte | 36:fb7f31e53ed9 | 225 | ble.gap().startAdvertising(); |
vazbyte | 36:fb7f31e53ed9 | 226 | } |
vazbyte | 31:d1ceadbc6c44 | 227 | |
vazbyte | 23:52e8e05df60c | 228 | void wakeup_event_cb() { |
vazbyte | 36:fb7f31e53ed9 | 229 | |
vazbyte | 23:52e8e05df60c | 230 | led != led; |
vazbyte | 38:7035f67d8105 | 231 | ::timestamp++; |
vazbyte | 35:daf134714cee | 232 | ::current_time = timestamp / time_cycle; |
vazbyte | 24:931eeb8a70fc | 233 | |
vazbyte | 29:6ba8491c1dab | 234 | if (countdown1_triggered) { |
vazbyte | 29:6ba8491c1dab | 235 | countdown--; |
vazbyte | 29:6ba8491c1dab | 236 | if (countdown == 0) { |
vazbyte | 29:6ba8491c1dab | 237 | countdown1_triggered = false; |
vazbyte | 29:6ba8491c1dab | 238 | } |
vazbyte | 29:6ba8491c1dab | 239 | } |
vazbyte | 29:6ba8491c1dab | 240 | if (countdown2_triggered) { |
vazbyte | 29:6ba8491c1dab | 241 | countdown--; |
vazbyte | 29:6ba8491c1dab | 242 | if (countdown == 0) { |
vazbyte | 29:6ba8491c1dab | 243 | countdown2_triggered = false; |
vazbyte | 29:6ba8491c1dab | 244 | } |
vazbyte | 29:6ba8491c1dab | 245 | } |
vazbyte | 31:d1ceadbc6c44 | 246 | |
vazbyte | 24:931eeb8a70fc | 247 | status1 = range1.get_distance(&distance1); |
vazbyte | 24:931eeb8a70fc | 248 | status2 = range2.get_distance(&distance2); |
vazbyte | 26:793d65b08afb | 249 | |
vazbyte | 27:903ec28ea7a0 | 250 | dist1 = format_dist(distance1); |
vazbyte | 27:903ec28ea7a0 | 251 | dist2 = format_dist(distance2); |
vazbyte | 31:d1ceadbc6c44 | 252 | |
vazbyte | 24:931eeb8a70fc | 253 | if (status1 == VL53L0X_ERROR_NONE) { |
vazbyte | 29:6ba8491c1dab | 254 | if (dist1 > DIST_MIN && dist1 < DIST_MAX) { |
vazbyte | 24:931eeb8a70fc | 255 | led1 = 0; |
vazbyte | 29:6ba8491c1dab | 256 | |
vazbyte | 29:6ba8491c1dab | 257 | if (!countdown1_triggered && !countdown2_triggered) { |
vazbyte | 29:6ba8491c1dab | 258 | countdown1_triggered = true; |
vazbyte | 29:6ba8491c1dab | 259 | countdown = cw; |
vazbyte | 34:1d3818f8c1a1 | 260 | } else if (countdown2_triggered && !range1_just_triggered) { |
vazbyte | 29:6ba8491c1dab | 261 | printf("STEP IN DETECTED\n"); |
vazbyte | 38:7035f67d8105 | 262 | if (master_connected) { |
vazbyte | 38:7035f67d8105 | 263 | send_packet(1, 0); |
vazbyte | 38:7035f67d8105 | 264 | } else { |
vazbyte | 38:7035f67d8105 | 265 | enqueue_packet(1, current_time, packet_queue); |
vazbyte | 38:7035f67d8105 | 266 | } |
vazbyte | 29:6ba8491c1dab | 267 | countdown2_triggered = false; |
vazbyte | 29:6ba8491c1dab | 268 | } |
vazbyte | 34:1d3818f8c1a1 | 269 | range1_just_triggered = true; |
vazbyte | 24:931eeb8a70fc | 270 | } |
vazbyte | 24:931eeb8a70fc | 271 | else { |
vazbyte | 24:931eeb8a70fc | 272 | led1 = 1; |
vazbyte | 34:1d3818f8c1a1 | 273 | range1_just_triggered = false; |
vazbyte | 24:931eeb8a70fc | 274 | } |
vazbyte | 24:931eeb8a70fc | 275 | } else { |
vazbyte | 24:931eeb8a70fc | 276 | led1 = 1; |
vazbyte | 34:1d3818f8c1a1 | 277 | range2_just_triggered = false; |
vazbyte | 24:931eeb8a70fc | 278 | } |
vazbyte | 24:931eeb8a70fc | 279 | if (status2 == VL53L0X_ERROR_NONE) { |
vazbyte | 29:6ba8491c1dab | 280 | if (dist2 > DIST_MIN && dist2 < DIST_MAX) { |
vazbyte | 27:903ec28ea7a0 | 281 | led2 = 0; |
vazbyte | 29:6ba8491c1dab | 282 | |
vazbyte | 29:6ba8491c1dab | 283 | if (!countdown1_triggered && !countdown2_triggered) { |
vazbyte | 29:6ba8491c1dab | 284 | countdown2_triggered = true; |
vazbyte | 29:6ba8491c1dab | 285 | countdown = cw; |
vazbyte | 34:1d3818f8c1a1 | 286 | } else if (countdown1_triggered && !range2_just_triggered) { |
vazbyte | 29:6ba8491c1dab | 287 | printf("STEP OUT DETECTED\n"); |
vazbyte | 38:7035f67d8105 | 288 | if (master_connected) { |
vazbyte | 39:067ba1a475a5 | 289 | send_packet(0, 0); |
vazbyte | 38:7035f67d8105 | 290 | } else { |
vazbyte | 39:067ba1a475a5 | 291 | enqueue_packet(0, current_time, packet_queue); |
vazbyte | 38:7035f67d8105 | 292 | } |
vazbyte | 29:6ba8491c1dab | 293 | countdown1_triggered = false; |
vazbyte | 29:6ba8491c1dab | 294 | } |
vazbyte | 34:1d3818f8c1a1 | 295 | |
vazbyte | 34:1d3818f8c1a1 | 296 | range2_just_triggered = true; |
vazbyte | 34:1d3818f8c1a1 | 297 | } |
vazbyte | 34:1d3818f8c1a1 | 298 | else { |
vazbyte | 34:1d3818f8c1a1 | 299 | led2 = 1; |
vazbyte | 34:1d3818f8c1a1 | 300 | range2_just_triggered = false; |
vazbyte | 27:903ec28ea7a0 | 301 | } |
vazbyte | 24:931eeb8a70fc | 302 | } else { |
vazbyte | 24:931eeb8a70fc | 303 | led2 = 1; |
vazbyte | 34:1d3818f8c1a1 | 304 | range2_just_triggered = false; |
vazbyte | 24:931eeb8a70fc | 305 | } |
vazbyte | 23:52e8e05df60c | 306 | } |
vazbyte | 31:d1ceadbc6c44 | 307 | |
andresag | 22:406127954d1f | 308 | int main(void) |
andresag | 22:406127954d1f | 309 | { |
vazbyte | 24:931eeb8a70fc | 310 | range1.init_sensor(range1_addr); |
vazbyte | 24:931eeb8a70fc | 311 | range2.init_sensor(range2_addr); |
vazbyte | 24:931eeb8a70fc | 312 | |
andresag | 22:406127954d1f | 313 | BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE); |
andresag | 22:406127954d1f | 314 | ble.init(bleInitComplete); |
andresag | 22:406127954d1f | 315 | |
vazbyte | 23:52e8e05df60c | 316 | Ticker ticker; |
vazbyte | 31:d1ceadbc6c44 | 317 | ticker.attach(wakeup_event_cb, 0.1 * TIME_SCALE); |
vazbyte | 24:931eeb8a70fc | 318 | |
vazbyte | 23:52e8e05df60c | 319 | while (ble.hasInitialized()) { |
vazbyte | 24:931eeb8a70fc | 320 | ble.waitForEvent(); |
mbedAustin | 2:e84c13abc479 | 321 | } |
andresag | 20:fcc752d401ec | 322 | } |