a hack to use buttons to fake a cube puck
Dependencies: MPU6050 Puck mbed
Fork of cube-puck by
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?
User | Revision | Line number | New 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 | } |