Stian Jensen / Mbed 2 deprecated cube-puck

yo

Dependencies:   MPU6050 Puck mbed

Fork of cube-puck by Nordic Pucks

main.cpp@6:fc59099597cd, 2014-07-31 (annotated)

Committer:
stiaje
Date:
Thu Jul 31 13:40:52 2014 +0000
Revision:
6:fc59099597cd
Parent:
4:6a2b306b6b41 
Remove unnecessary UUID wrapper call

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aleksanb 0:449ee9595cf6 1 #include "MPU6050.h"
aleksanb 2:b9b42ff80e9a 2 #include <math.h>
aleksanb 0:449ee9595cf6 3
sigveseb 3:6a7310ea51f7 4 #define LOG_LEVEL_INFO
sigveseb 3:6a7310ea51f7 5 #include "Puck.h"
sigveseb 3:6a7310ea51f7 6
sigveseb 3:6a7310ea51f7 7 Puck* puck = &Puck::getPuck();
sigveseb 3:6a7310ea51f7 8
stiaje 6:fc59099597cd 9 const UUID CUBE_SERVICE_UUID = stringToUUID("bftj cube ");
stiaje 6:fc59099597cd 10 const UUID DIRECTION_UUID = stringToUUID("bftj cube dirctn");
aleksanb 0:449ee9595cf6 11
aleksanb 0:449ee9595cf6 12 enum Direction {
aleksanb 0:449ee9595cf6 13 UP,
aleksanb 0:449ee9595cf6 14 DOWN,
aleksanb 0:449ee9595cf6 15 LEFT,
aleksanb 0:449ee9595cf6 16 RIGHT,
aleksanb 0:449ee9595cf6 17 FRONT,
aleksanb 0:449ee9595cf6 18 BACK,
aleksanb 0:449ee9595cf6 19 UNDEFINED
aleksanb 0:449ee9595cf6 20 };
aleksanb 0:449ee9595cf6 21
aleksanb 0:449ee9595cf6 22 const static int16_t ACCELERATION_EXITATION_THRESHOLD = 15000;
aleksanb 0:449ee9595cf6 23
aleksanb 0:449ee9595cf6 24 MPU6050 mpu;
aleksanb 0:449ee9595cf6 25
aleksanb 0:449ee9595cf6 26 int16_t ax, ay, az;
aleksanb 0:449ee9595cf6 27 int16_t gx, gy, gz;
aleksanb 0:449ee9595cf6 28
aleksanb 0:449ee9595cf6 29 Direction direction = UNDEFINED;
aleksanb 0:449ee9595cf6 30
sigveseb 3:6a7310ea51f7 31 void log_direction(Direction direction) {
aleksanb 2:b9b42ff80e9a 32 switch(direction) {
sigveseb 3:6a7310ea51f7 33 case UP: LOG_INFO("Direction UP\n"); break;
sigveseb 3:6a7310ea51f7 34 case DOWN: LOG_INFO("Direction DOWN\n"); break;
sigveseb 3:6a7310ea51f7 35 case LEFT: LOG_INFO("Direction LEFT\n"); break;
sigveseb 3:6a7310ea51f7 36 case RIGHT: LOG_INFO("Direction RIGHT\n"); break;
sigveseb 3:6a7310ea51f7 37 case BACK: LOG_INFO("Direction BACK\n"); break;
sigveseb 3:6a7310ea51f7 38 case FRONT: LOG_INFO("Direction FRONT\n"); break;
sigveseb 3:6a7310ea51f7 39 default: LOG_INFO("Direction UNSET\n"); break;
aleksanb 0:449ee9595cf6 40 }
aleksanb 0:449ee9595cf6 41 }
aleksanb 0:449ee9595cf6 42
sigveseb 3:6a7310ea51f7 43 int16_t direction_if_exited(int16_t acceleration) {
aleksanb 0:449ee9595cf6 44 if (acceleration > ACCELERATION_EXITATION_THRESHOLD) {
aleksanb 0:449ee9595cf6 45 return 1;
sigveseb 3:6a7310ea51f7 46 }
sigveseb 3:6a7310ea51f7 47 if (acceleration < -ACCELERATION_EXITATION_THRESHOLD) {
aleksanb 0:449ee9595cf6 48 return -1;
aleksanb 0:449ee9595cf6 49 }
sigveseb 3:6a7310ea51f7 50 return 0;
aleksanb 0:449ee9595cf6 51 }
aleksanb 0:449ee9595cf6 52
sigveseb 3:6a7310ea51f7 53 void updateCubeDirection(void) {
aleksanb 0:449ee9595cf6 54 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
aleksanb 2:b9b42ff80e9a 55
aleksanb 0:449ee9595cf6 56 int16_t x = direction_if_exited(ax);
aleksanb 0:449ee9595cf6 57 int16_t y = direction_if_exited(ay);
aleksanb 0:449ee9595cf6 58 int16_t z = direction_if_exited(az);
aleksanb 2:b9b42ff80e9a 59
aleksanb 0:449ee9595cf6 60 int16_t sum = abs(x) + abs(y) + abs(z);
aleksanb 2:b9b42ff80e9a 61 if (sum != 1) {
aleksanb 0:449ee9595cf6 62 return;
aleksanb 0:449ee9595cf6 63 }
aleksanb 2:b9b42ff80e9a 64
sigveseb 3:6a7310ea51f7 65 Direction new_direction = UNDEFINED;
aleksanb 0:449ee9595cf6 66 if (z == 1) {
aleksanb 2:b9b42ff80e9a 67 new_direction = UP;
aleksanb 0:449ee9595cf6 68 } else if (z == -1) {
aleksanb 2:b9b42ff80e9a 69 new_direction = DOWN;
aleksanb 0:449ee9595cf6 70 } else if (y == 1) {
aleksanb 2:b9b42ff80e9a 71 new_direction = LEFT;
aleksanb 0:449ee9595cf6 72 } else if (y == -1) {
aleksanb 2:b9b42ff80e9a 73 new_direction = RIGHT;
aleksanb 0:449ee9595cf6 74 } else if (x == 1) {
aleksanb 2:b9b42ff80e9a 75 new_direction = BACK;
aleksanb 0:449ee9595cf6 76 } else if (x == -1) {
aleksanb 2:b9b42ff80e9a 77 new_direction = FRONT;
aleksanb 0:449ee9595cf6 78 }
aleksanb 2:b9b42ff80e9a 79
aleksanb 2:b9b42ff80e9a 80 if (direction == new_direction) {
aleksanb 2:b9b42ff80e9a 81 return;
aleksanb 2:b9b42ff80e9a 82 }
aleksanb 2:b9b42ff80e9a 83
aleksanb 2:b9b42ff80e9a 84 direction = new_direction;
aleksanb 2:b9b42ff80e9a 85
sigveseb 3:6a7310ea51f7 86 log_direction(direction);
sigveseb 3:6a7310ea51f7 87 uint8_t directionAsInteger = direction;
sigveseb 4:6a2b306b6b41 88 int length = 1;
sigveseb 4:6a2b306b6b41 89 puck->updateCharacteristicValue(DIRECTION_UUID, &directionAsInteger, length);
aleksanb 2:b9b42ff80e9a 90 }
aleksanb 2:b9b42ff80e9a 91
aleksanb 2:b9b42ff80e9a 92
sigveseb 3:6a7310ea51f7 93 int main() {
sigveseb 3:6a7310ea51f7 94
aleksanb 0:449ee9595cf6 95
sigveseb 3:6a7310ea51f7 96 LOG_VERBOSE("MPU6050 test startup:\n");
aleksanb 0:449ee9595cf6 97
aleksanb 0:449ee9595cf6 98 mpu.initialize();
sigveseb 3:6a7310ea51f7 99 LOG_VERBOSE("TestConnection\n");
aleksanb 2:b9b42ff80e9a 100
aleksanb 2:b9b42ff80e9a 101 if (mpu.testConnection()) {
sigveseb 3:6a7310ea51f7 102 LOG_INFO("MPU initialized.\n");
aleksanb 2:b9b42ff80e9a 103 } else {
sigveseb 3:6a7310ea51f7 104 LOG_ERROR("MPU not properly initialized!\n");
aleksanb 0:449ee9595cf6 105 }
aleksanb 0:449ee9595cf6 106
sigveseb 4:6a2b306b6b41 107 int characteristicValueLength = 1;
sigveseb 3:6a7310ea51f7 108 puck->addCharacteristic(
sigveseb 3:6a7310ea51f7 109 CUBE_SERVICE_UUID,
sigveseb 3:6a7310ea51f7 110 DIRECTION_UUID,
sigveseb 4:6a2b306b6b41 111 characteristicValueLength,
sigveseb 3:6a7310ea51f7 112 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
sigveseb 3:6a7310ea51f7 113
sigveseb 3:6a7310ea51f7 114 puck->init(0xC0BE);
sigveseb 4:6a2b306b6b41 115
sigveseb 4:6a2b306b6b41 116
sigveseb 4:6a2b306b6b41 117 Ticker ticker;
sigveseb 4:6a2b306b6b41 118 ticker.attach(updateCubeDirection, 1);
sigveseb 4:6a2b306b6b41 119 LOG_INFO("Started listening to orientation changes.\n");
sigveseb 3:6a7310ea51f7 120
sigveseb 3:6a7310ea51f7 121 while(puck->drive());
aleksanb 0:449ee9595cf6 122 }