Aleksander Vognild Burkow / Mbed 2 deprecated IMU

The cube puck updates its gatt attributes on rotation. Listen in!

Dependencies:   BLE_API MPU6050 mbed nRF51822

Fork of cube-puck by Nordic Pucks

main.cpp@0:449ee9595cf6, 2014-07-03 (annotated)

Committer:
aleksanb
Date:
Thu Jul 03 11:25:12 2014 +0000
Revision:
0:449ee9595cf6
Child:
2:b9b42ff80e9a
Initial commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aleksanb 0:449ee9595cf6 1 #include "mbed.h"
aleksanb 0:449ee9595cf6 2 #include "BLEDevice.h"
aleksanb 0:449ee9595cf6 3 #include "nRF51822n.h"
aleksanb 0:449ee9595cf6 4 #include "MPU6050.h"
aleksanb 0:449ee9595cf6 5 #include <math.h>
aleksanb 0:449ee9595cf6 6
aleksanb 0:449ee9595cf6 7 #define DEBUG 1
aleksanb 0:449ee9595cf6 8
aleksanb 0:449ee9595cf6 9 enum Direction {
aleksanb 0:449ee9595cf6 10 UP,
aleksanb 0:449ee9595cf6 11 DOWN,
aleksanb 0:449ee9595cf6 12 LEFT,
aleksanb 0:449ee9595cf6 13 RIGHT,
aleksanb 0:449ee9595cf6 14 FRONT,
aleksanb 0:449ee9595cf6 15 BACK,
aleksanb 0:449ee9595cf6 16 UNDEFINED
aleksanb 0:449ee9595cf6 17 };
aleksanb 0:449ee9595cf6 18
aleksanb 0:449ee9595cf6 19 Serial pc(USBTX, USBRX);
aleksanb 0:449ee9595cf6 20
aleksanb 0:449ee9595cf6 21 BLEDevice ble;
aleksanb 0:449ee9595cf6 22
aleksanb 0:449ee9595cf6 23 const static int16_t ACCELERATION_EXITATION_THRESHOLD = 15000;
aleksanb 0:449ee9595cf6 24 const static uint8_t beaconPayload[] = {
aleksanb 0:449ee9595cf6 25 0x00, 0x4C, // Company identifier code (0x004C == Apple)
aleksanb 0:449ee9595cf6 26 0x02, // ID
aleksanb 0:449ee9595cf6 27 0x15, // length of the remaining payload
aleksanb 0:449ee9595cf6 28 0xE2, 0x0A, 0x39, 0xF4, 0x73, 0xF5, 0x4B, 0xC4, // UUID
aleksanb 0:449ee9595cf6 29 0xA1, 0x2F, 0x17, 0xD1, 0xAD, 0x07, 0xA9, 0x61,
aleksanb 0:449ee9595cf6 30 0x13, 0x37, // the major value to differenciate a location
aleksanb 0:449ee9595cf6 31 0xFA, 0xCE, // the minor value to differenciate a location
aleksanb 0:449ee9595cf6 32 0xC8 // 2's complement of the Tx power (-56dB)
aleksanb 0:449ee9595cf6 33 };
aleksanb 0:449ee9595cf6 34
aleksanb 0:449ee9595cf6 35 extern GattService cube_service;
aleksanb 0:449ee9595cf6 36 extern GattCharacteristic directionCharacteristic;
aleksanb 0:449ee9595cf6 37 extern uint8_t direction_data[1];
aleksanb 0:449ee9595cf6 38
aleksanb 0:449ee9595cf6 39 MPU6050 mpu;
aleksanb 0:449ee9595cf6 40
aleksanb 0:449ee9595cf6 41 int16_t ax, ay, az;
aleksanb 0:449ee9595cf6 42 int16_t gx, gy, gz;
aleksanb 0:449ee9595cf6 43
aleksanb 0:449ee9595cf6 44 Direction direction = UNDEFINED;
aleksanb 0:449ee9595cf6 45
aleksanb 0:449ee9595cf6 46 void log_direction(void)
aleksanb 0:449ee9595cf6 47 {
aleksanb 0:449ee9595cf6 48 switch(direction)
aleksanb 0:449ee9595cf6 49 {
aleksanb 0:449ee9595cf6 50 case UP:
aleksanb 0:449ee9595cf6 51 pc.printf("Direction UP\n");
aleksanb 0:449ee9595cf6 52 break;
aleksanb 0:449ee9595cf6 53
aleksanb 0:449ee9595cf6 54 case DOWN:
aleksanb 0:449ee9595cf6 55 pc.printf("Direction DOWN\n");
aleksanb 0:449ee9595cf6 56 break;
aleksanb 0:449ee9595cf6 57
aleksanb 0:449ee9595cf6 58 case LEFT:
aleksanb 0:449ee9595cf6 59 pc.printf("Direction LEFT\n");
aleksanb 0:449ee9595cf6 60 break;
aleksanb 0:449ee9595cf6 61
aleksanb 0:449ee9595cf6 62 case RIGHT:
aleksanb 0:449ee9595cf6 63 pc.printf("Direction RIGHT\n");
aleksanb 0:449ee9595cf6 64 break;
aleksanb 0:449ee9595cf6 65
aleksanb 0:449ee9595cf6 66 case BACK:
aleksanb 0:449ee9595cf6 67 pc.printf("Direction BACK\n");
aleksanb 0:449ee9595cf6 68 break;
aleksanb 0:449ee9595cf6 69
aleksanb 0:449ee9595cf6 70 case FRONT:
aleksanb 0:449ee9595cf6 71 pc.printf("Direction FRONT\n");
aleksanb 0:449ee9595cf6 72 break;
aleksanb 0:449ee9595cf6 73
aleksanb 0:449ee9595cf6 74 default:
aleksanb 0:449ee9595cf6 75 pc.printf("Direction UNSET\n");
aleksanb 0:449ee9595cf6 76 break;
aleksanb 0:449ee9595cf6 77 }
aleksanb 0:449ee9595cf6 78 }
aleksanb 0:449ee9595cf6 79
aleksanb 0:449ee9595cf6 80 int16_t inline direction_if_exited(int16_t acceleration) {
aleksanb 0:449ee9595cf6 81 if (acceleration > ACCELERATION_EXITATION_THRESHOLD) {
aleksanb 0:449ee9595cf6 82 return 1;
aleksanb 0:449ee9595cf6 83 } else if (acceleration < -ACCELERATION_EXITATION_THRESHOLD) {
aleksanb 0:449ee9595cf6 84 return -1;
aleksanb 0:449ee9595cf6 85 } else {
aleksanb 0:449ee9595cf6 86 return 0;
aleksanb 0:449ee9595cf6 87 }
aleksanb 0:449ee9595cf6 88 }
aleksanb 0:449ee9595cf6 89
aleksanb 0:449ee9595cf6 90 void update_cube_direction(void)
aleksanb 0:449ee9595cf6 91 {
aleksanb 0:449ee9595cf6 92 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
aleksanb 0:449ee9595cf6 93
aleksanb 0:449ee9595cf6 94 int16_t x = direction_if_exited(ax);
aleksanb 0:449ee9595cf6 95 int16_t y = direction_if_exited(ay);
aleksanb 0:449ee9595cf6 96 int16_t z = direction_if_exited(az);
aleksanb 0:449ee9595cf6 97
aleksanb 0:449ee9595cf6 98 int16_t sum = abs(x) + abs(y) + abs(z);
aleksanb 0:449ee9595cf6 99 if (sum == 0 || sum != 1) {
aleksanb 0:449ee9595cf6 100 return;
aleksanb 0:449ee9595cf6 101 }
aleksanb 0:449ee9595cf6 102
aleksanb 0:449ee9595cf6 103 if (z == 1) {
aleksanb 0:449ee9595cf6 104 direction = UP;
aleksanb 0:449ee9595cf6 105 } else if (z == -1) {
aleksanb 0:449ee9595cf6 106 direction = DOWN;
aleksanb 0:449ee9595cf6 107 } else if (y == 1) {
aleksanb 0:449ee9595cf6 108 direction = LEFT;
aleksanb 0:449ee9595cf6 109 } else if (y == -1) {
aleksanb 0:449ee9595cf6 110 direction = RIGHT;
aleksanb 0:449ee9595cf6 111 } else if (x == 1) {
aleksanb 0:449ee9595cf6 112 direction = BACK;
aleksanb 0:449ee9595cf6 113 } else if (x == -1) {
aleksanb 0:449ee9595cf6 114 direction = FRONT;
aleksanb 0:449ee9595cf6 115 }
aleksanb 0:449ee9595cf6 116
aleksanb 0:449ee9595cf6 117 #if DEBUG
aleksanb 0:449ee9595cf6 118 log_direction();
aleksanb 0:449ee9595cf6 119 #endif
aleksanb 0:449ee9595cf6 120 }
aleksanb 0:449ee9595cf6 121
aleksanb 0:449ee9595cf6 122 void update_direction_characteristic(void)
aleksanb 0:449ee9595cf6 123 {
aleksanb 0:449ee9595cf6 124 direction_data[0] = direction;
aleksanb 0:449ee9595cf6 125 ble.updateCharacteristicValue(directionCharacteristic.getHandle(),
aleksanb 0:449ee9595cf6 126 direction_data,
aleksanb 0:449ee9595cf6 127 sizeof(direction_data));
aleksanb 0:449ee9595cf6 128 #if DEBUG
aleksanb 0:449ee9595cf6 129 pc.printf("Updated gatt characteristic\n");
aleksanb 0:449ee9595cf6 130 #endif
aleksanb 0:449ee9595cf6 131 }
aleksanb 0:449ee9595cf6 132
aleksanb 0:449ee9595cf6 133 void disconnectionCallback(void)
aleksanb 0:449ee9595cf6 134 {
aleksanb 0:449ee9595cf6 135 pc.printf("Disconnected!\n");
aleksanb 0:449ee9595cf6 136 pc.printf("Restarting the advertising process\n");
aleksanb 0:449ee9595cf6 137 ble.startAdvertising();
aleksanb 0:449ee9595cf6 138 }
aleksanb 0:449ee9595cf6 139
aleksanb 0:449ee9595cf6 140 void setup_ble(void)
aleksanb 0:449ee9595cf6 141 {
aleksanb 0:449ee9595cf6 142 ble.init();
aleksanb 0:449ee9595cf6 143 ble.onDisconnection(disconnectionCallback);
aleksanb 0:449ee9595cf6 144
aleksanb 0:449ee9595cf6 145 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
aleksanb 0:449ee9595cf6 146 ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
aleksanb 0:449ee9595cf6 147 beaconPayload, sizeof(beaconPayload));
aleksanb 0:449ee9595cf6 148
aleksanb 0:449ee9595cf6 149 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
aleksanb 0:449ee9595cf6 150 ble.setAdvertisingInterval(160);
aleksanb 0:449ee9595cf6 151
aleksanb 0:449ee9595cf6 152 ble.startAdvertising();
aleksanb 0:449ee9595cf6 153
aleksanb 0:449ee9595cf6 154 ble.addService(cube_service);
aleksanb 0:449ee9595cf6 155
aleksanb 0:449ee9595cf6 156 pc.printf("BLE set up and running\n");
aleksanb 0:449ee9595cf6 157 }
aleksanb 0:449ee9595cf6 158
aleksanb 0:449ee9595cf6 159 int main()
aleksanb 0:449ee9595cf6 160 {
aleksanb 0:449ee9595cf6 161 setup_ble();
aleksanb 0:449ee9595cf6 162 pc.printf("MPU6050 test startup:\n");
aleksanb 0:449ee9595cf6 163
aleksanb 0:449ee9595cf6 164 mpu.initialize();
aleksanb 0:449ee9595cf6 165 pc.printf("TestConnection\n");
aleksanb 0:449ee9595cf6 166
aleksanb 0:449ee9595cf6 167 if (mpu.testConnection())
aleksanb 0:449ee9595cf6 168 {
aleksanb 0:449ee9595cf6 169 pc.printf("MPU success\n");
aleksanb 0:449ee9595cf6 170 }
aleksanb 0:449ee9595cf6 171 else
aleksanb 0:449ee9595cf6 172 {
aleksanb 0:449ee9595cf6 173 pc.printf("MPU error\n");
aleksanb 0:449ee9595cf6 174 }
aleksanb 0:449ee9595cf6 175
aleksanb 0:449ee9595cf6 176 while(1)
aleksanb 0:449ee9595cf6 177 {
aleksanb 0:449ee9595cf6 178 ble.waitForEvent();
aleksanb 0:449ee9595cf6 179 update_cube_direction();
aleksanb 0:449ee9595cf6 180 update_direction_characteristic();
aleksanb 0:449ee9595cf6 181 }
aleksanb 0:449ee9595cf6 182 }