![](/media/cache/profiles/909f269e0b3ffbd61d906f9b25eecaf6.jpg.50x50_q85.jpg)
a hack to use buttons to fake a cube puck
Dependencies: MPU6050 Puck mbed
Fork of cube-puck by
main.cpp@10:3d708495b7a0, 2015-02-27 (annotated)
- Committer:
- stiaje
- Date:
- Fri Feb 27 18:59:33 2015 +0000
- Revision:
- 10:3d708495b7a0
- Parent:
- 8:4244572015c6
- Parent:
- 6:fc59099597cd
- Child:
- 12:a7051fc219b8
Merge 'remove unnecessary UUID call'
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cristea | 7:4f2aaa06ff44 | 1 | /** |
cristea | 7:4f2aaa06ff44 | 2 | * Copyright 2014 Nordic Semiconductor |
cristea | 7:4f2aaa06ff44 | 3 | * |
cristea | 7:4f2aaa06ff44 | 4 | * Licensed under the Apache License, Version 2.0 (the "License"); |
cristea | 7:4f2aaa06ff44 | 5 | * you may not use this file except in compliance with the License. |
cristea | 7:4f2aaa06ff44 | 6 | * You may obtain a copy of the License at |
cristea | 7:4f2aaa06ff44 | 7 | * |
cristea | 7:4f2aaa06ff44 | 8 | * http://www.apache.org/licenses/LICENSE-2.0 |
cristea | 7:4f2aaa06ff44 | 9 | * |
cristea | 7:4f2aaa06ff44 | 10 | * Unless required by applicable law or agreed to in writing, software |
cristea | 7:4f2aaa06ff44 | 11 | * distributed under the License is distributed on an "AS IS" BASIS, |
cristea | 7:4f2aaa06ff44 | 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
cristea | 7:4f2aaa06ff44 | 13 | * See the License for the specific language governing permissions and |
cristea | 7:4f2aaa06ff44 | 14 | * limitations under the License |
cristea | 7:4f2aaa06ff44 | 15 | */ |
cristea | 7:4f2aaa06ff44 | 16 | |
aleksanb | 0:449ee9595cf6 | 17 | #include "MPU6050.h" |
aleksanb | 2:b9b42ff80e9a | 18 | #include <math.h> |
aleksanb | 0:449ee9595cf6 | 19 | |
sigveseb | 3:6a7310ea51f7 | 20 | #include "Puck.h" |
sigveseb | 3:6a7310ea51f7 | 21 | |
sigveseb | 3:6a7310ea51f7 | 22 | Puck* puck = &Puck::getPuck(); |
sigveseb | 3:6a7310ea51f7 | 23 | |
stiaje | 6:fc59099597cd | 24 | const UUID CUBE_SERVICE_UUID = stringToUUID("bftj cube "); |
stiaje | 6:fc59099597cd | 25 | const UUID DIRECTION_UUID = stringToUUID("bftj cube dirctn"); |
aleksanb | 0:449ee9595cf6 | 26 | |
aleksanb | 0:449ee9595cf6 | 27 | enum Direction { |
aleksanb | 0:449ee9595cf6 | 28 | UP, |
aleksanb | 0:449ee9595cf6 | 29 | DOWN, |
aleksanb | 0:449ee9595cf6 | 30 | LEFT, |
aleksanb | 0:449ee9595cf6 | 31 | RIGHT, |
aleksanb | 0:449ee9595cf6 | 32 | FRONT, |
aleksanb | 0:449ee9595cf6 | 33 | BACK, |
aleksanb | 0:449ee9595cf6 | 34 | UNDEFINED |
aleksanb | 0:449ee9595cf6 | 35 | }; |
aleksanb | 0:449ee9595cf6 | 36 | |
aleksanb | 0:449ee9595cf6 | 37 | const static int16_t ACCELERATION_EXITATION_THRESHOLD = 15000; |
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 | |
sigveseb | 3:6a7310ea51f7 | 46 | void log_direction(Direction direction) { |
aleksanb | 2:b9b42ff80e9a | 47 | switch(direction) { |
sigveseb | 3:6a7310ea51f7 | 48 | case UP: LOG_INFO("Direction UP\n"); break; |
sigveseb | 3:6a7310ea51f7 | 49 | case DOWN: LOG_INFO("Direction DOWN\n"); break; |
sigveseb | 3:6a7310ea51f7 | 50 | case LEFT: LOG_INFO("Direction LEFT\n"); break; |
sigveseb | 3:6a7310ea51f7 | 51 | case RIGHT: LOG_INFO("Direction RIGHT\n"); break; |
sigveseb | 3:6a7310ea51f7 | 52 | case BACK: LOG_INFO("Direction BACK\n"); break; |
sigveseb | 3:6a7310ea51f7 | 53 | case FRONT: LOG_INFO("Direction FRONT\n"); break; |
sigveseb | 3:6a7310ea51f7 | 54 | default: LOG_INFO("Direction UNSET\n"); break; |
aleksanb | 0:449ee9595cf6 | 55 | } |
aleksanb | 0:449ee9595cf6 | 56 | } |
aleksanb | 0:449ee9595cf6 | 57 | |
sigveseb | 3:6a7310ea51f7 | 58 | int16_t direction_if_exited(int16_t acceleration) { |
aleksanb | 0:449ee9595cf6 | 59 | if (acceleration > ACCELERATION_EXITATION_THRESHOLD) { |
aleksanb | 0:449ee9595cf6 | 60 | return 1; |
sigveseb | 3:6a7310ea51f7 | 61 | } |
sigveseb | 3:6a7310ea51f7 | 62 | if (acceleration < -ACCELERATION_EXITATION_THRESHOLD) { |
aleksanb | 0:449ee9595cf6 | 63 | return -1; |
aleksanb | 0:449ee9595cf6 | 64 | } |
sigveseb | 3:6a7310ea51f7 | 65 | return 0; |
aleksanb | 0:449ee9595cf6 | 66 | } |
aleksanb | 0:449ee9595cf6 | 67 | |
sigveseb | 3:6a7310ea51f7 | 68 | void updateCubeDirection(void) { |
sigveseb | 8:4244572015c6 | 69 | |
sigveseb | 8:4244572015c6 | 70 | if(!mpu.testConnection()) { |
sigveseb | 8:4244572015c6 | 71 | LOG_ERROR("MPU DIED! Resetting...\n"); |
sigveseb | 8:4244572015c6 | 72 | mpu.reset(); |
sigveseb | 8:4244572015c6 | 73 | mpu.initialize(); |
sigveseb | 8:4244572015c6 | 74 | LOG_ERROR("Reset complete.\n"); |
sigveseb | 8:4244572015c6 | 75 | return; |
sigveseb | 8:4244572015c6 | 76 | } |
sigveseb | 8:4244572015c6 | 77 | |
aleksanb | 0:449ee9595cf6 | 78 | mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
aleksanb | 2:b9b42ff80e9a | 79 | |
aleksanb | 0:449ee9595cf6 | 80 | int16_t x = direction_if_exited(ax); |
aleksanb | 0:449ee9595cf6 | 81 | int16_t y = direction_if_exited(ay); |
aleksanb | 0:449ee9595cf6 | 82 | int16_t z = direction_if_exited(az); |
aleksanb | 2:b9b42ff80e9a | 83 | |
aleksanb | 0:449ee9595cf6 | 84 | int16_t sum = abs(x) + abs(y) + abs(z); |
aleksanb | 2:b9b42ff80e9a | 85 | if (sum != 1) { |
aleksanb | 0:449ee9595cf6 | 86 | return; |
aleksanb | 0:449ee9595cf6 | 87 | } |
aleksanb | 2:b9b42ff80e9a | 88 | |
sigveseb | 3:6a7310ea51f7 | 89 | Direction new_direction = UNDEFINED; |
aleksanb | 0:449ee9595cf6 | 90 | if (z == 1) { |
aleksanb | 2:b9b42ff80e9a | 91 | new_direction = UP; |
aleksanb | 0:449ee9595cf6 | 92 | } else if (z == -1) { |
aleksanb | 2:b9b42ff80e9a | 93 | new_direction = DOWN; |
aleksanb | 0:449ee9595cf6 | 94 | } else if (y == 1) { |
aleksanb | 2:b9b42ff80e9a | 95 | new_direction = LEFT; |
aleksanb | 0:449ee9595cf6 | 96 | } else if (y == -1) { |
aleksanb | 2:b9b42ff80e9a | 97 | new_direction = RIGHT; |
aleksanb | 0:449ee9595cf6 | 98 | } else if (x == 1) { |
aleksanb | 2:b9b42ff80e9a | 99 | new_direction = BACK; |
aleksanb | 0:449ee9595cf6 | 100 | } else if (x == -1) { |
aleksanb | 2:b9b42ff80e9a | 101 | new_direction = FRONT; |
aleksanb | 0:449ee9595cf6 | 102 | } |
aleksanb | 2:b9b42ff80e9a | 103 | |
aleksanb | 2:b9b42ff80e9a | 104 | if (direction == new_direction) { |
aleksanb | 2:b9b42ff80e9a | 105 | return; |
aleksanb | 2:b9b42ff80e9a | 106 | } |
aleksanb | 2:b9b42ff80e9a | 107 | |
aleksanb | 2:b9b42ff80e9a | 108 | direction = new_direction; |
aleksanb | 2:b9b42ff80e9a | 109 | |
sigveseb | 3:6a7310ea51f7 | 110 | log_direction(direction); |
sigveseb | 3:6a7310ea51f7 | 111 | uint8_t directionAsInteger = direction; |
sigveseb | 4:6a2b306b6b41 | 112 | int length = 1; |
sigveseb | 4:6a2b306b6b41 | 113 | puck->updateCharacteristicValue(DIRECTION_UUID, &directionAsInteger, length); |
aleksanb | 2:b9b42ff80e9a | 114 | } |
aleksanb | 2:b9b42ff80e9a | 115 | |
aleksanb | 2:b9b42ff80e9a | 116 | |
sigveseb | 3:6a7310ea51f7 | 117 | int main() { |
sigveseb | 3:6a7310ea51f7 | 118 | |
aleksanb | 0:449ee9595cf6 | 119 | |
sigveseb | 3:6a7310ea51f7 | 120 | LOG_VERBOSE("MPU6050 test startup:\n"); |
aleksanb | 0:449ee9595cf6 | 121 | |
aleksanb | 0:449ee9595cf6 | 122 | mpu.initialize(); |
sigveseb | 3:6a7310ea51f7 | 123 | LOG_VERBOSE("TestConnection\n"); |
aleksanb | 2:b9b42ff80e9a | 124 | |
aleksanb | 2:b9b42ff80e9a | 125 | if (mpu.testConnection()) { |
sigveseb | 3:6a7310ea51f7 | 126 | LOG_INFO("MPU initialized.\n"); |
aleksanb | 2:b9b42ff80e9a | 127 | } else { |
sigveseb | 3:6a7310ea51f7 | 128 | LOG_ERROR("MPU not properly initialized!\n"); |
aleksanb | 0:449ee9595cf6 | 129 | } |
aleksanb | 0:449ee9595cf6 | 130 | |
sigveseb | 4:6a2b306b6b41 | 131 | int characteristicValueLength = 1; |
sigveseb | 3:6a7310ea51f7 | 132 | puck->addCharacteristic( |
sigveseb | 3:6a7310ea51f7 | 133 | CUBE_SERVICE_UUID, |
sigveseb | 3:6a7310ea51f7 | 134 | DIRECTION_UUID, |
sigveseb | 4:6a2b306b6b41 | 135 | characteristicValueLength, |
sigveseb | 3:6a7310ea51f7 | 136 | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); |
sigveseb | 3:6a7310ea51f7 | 137 | |
cristea | 7:4f2aaa06ff44 | 138 | puck->init(0xC1BE); |
sigveseb | 4:6a2b306b6b41 | 139 | |
sigveseb | 4:6a2b306b6b41 | 140 | |
sigveseb | 4:6a2b306b6b41 | 141 | Ticker ticker; |
cristea | 7:4f2aaa06ff44 | 142 | ticker.attach(updateCubeDirection, 0.2); |
sigveseb | 4:6a2b306b6b41 | 143 | LOG_INFO("Started listening to orientation changes.\n"); |
sigveseb | 3:6a7310ea51f7 | 144 | |
sigveseb | 3:6a7310ea51f7 | 145 | while(puck->drive()); |
aleksanb | 0:449ee9595cf6 | 146 | } |