main.cpp

00001 #include "mbed.h"
00002 #include "BLEDevice.h"
00003 #include "nRF51822n.h"
00004 #include "MPU6050.h"
00005 #include <math.h>
00006 
00007 #define DEBUG 1
00008 
00009 enum Direction {
00010     UP,
00011     DOWN,
00012     LEFT,
00013     RIGHT,
00014     FRONT,
00015     BACK,
00016     UNDEFINED
00017 };
00018 
00019 Serial pc(USBTX, USBRX);
00020 
00021 BLEDevice ble;
00022 
00023 const static int16_t ACCELERATION_EXITATION_THRESHOLD = 15000;
00024 const static uint8_t beaconPayload[] = {
00025     0x00, 0x4C, // Company identifier code (0x004C == Apple)
00026     0x02,       // ID
00027     0x15,       // length of the remaining payload
00028     0xE2, 0x0A, 0x39, 0xF4, 0x73, 0xF5, 0x4B, 0xC4, // UUID
00029     0xA1, 0x2F, 0x17, 0xD1, 0xAD, 0x07, 0xA9, 0x61,
00030     0x13, 0x37, // the major value to differenciate a location
00031     0xC0, 0xBE, // the minor value to differenciate a location
00032     0xC8        // 2's complement of the Tx power (-56dB)
00033 };
00034 
00035 extern GattService cube_service;
00036 extern GattCharacteristic directionCharacteristic;
00037 extern uint8_t direction_data[1];
00038 
00039 MPU6050 mpu;
00040 
00041 int16_t ax, ay, az;
00042 int16_t gx, gy, gz;
00043 
00044 Direction direction = UNDEFINED;
00045 
00046 void log_direction(void)
00047 {
00048     switch(direction) {
00049         case UP:
00050             pc.printf("Direction UP\n");
00051             break;
00052 
00053         case DOWN:
00054             pc.printf("Direction DOWN\n");
00055             break;
00056 
00057         case LEFT:
00058             pc.printf("Direction LEFT\n");
00059             break;
00060 
00061         case RIGHT:
00062             pc.printf("Direction RIGHT\n");
00063             break;
00064 
00065         case BACK:
00066             pc.printf("Direction BACK\n");
00067             break;
00068 
00069         case FRONT:
00070             pc.printf("Direction FRONT\n");
00071             break;
00072 
00073         default:
00074             pc.printf("Direction UNSET\n");
00075             break;
00076     }
00077 }
00078 
00079 int16_t direction_if_exited(int16_t acceleration)
00080 {
00081     if (acceleration > ACCELERATION_EXITATION_THRESHOLD) {
00082         return 1;
00083     } else if (acceleration < -ACCELERATION_EXITATION_THRESHOLD) {
00084         return -1;
00085     } else {
00086         return 0;
00087     }
00088 }
00089 
00090 void update_direction_characteristic(void)
00091 {
00092     direction_data[0] = direction;
00093     ble.updateCharacteristicValue(directionCharacteristic.getHandle(),
00094                                   direction_data,
00095                                   sizeof(direction_data));
00096 #if DEBUG
00097     //pc.printf("Updated gatt characteristic\n");
00098 #endif
00099 }
00100 
00101 void update_cube_direction(void)
00102 {
00103     mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00104 
00105     int16_t x = direction_if_exited(ax);
00106     int16_t y = direction_if_exited(ay);
00107     int16_t z = direction_if_exited(az);
00108 
00109     int16_t sum = abs(x) + abs(y) + abs(z);
00110     if (sum != 1) {
00111         return;
00112     }
00113 
00114     Direction new_direction;
00115     if (z == 1) {
00116         new_direction = UP;
00117     } else if (z == -1) {
00118         new_direction = DOWN;
00119     } else if (y == 1) {
00120         new_direction = LEFT;
00121     } else if (y == -1) {
00122         new_direction = RIGHT;
00123     } else if (x == 1) {
00124         new_direction = BACK;
00125     } else if (x == -1) {
00126         new_direction = FRONT;
00127     }
00128 
00129     if (direction == new_direction) {
00130         return;
00131     }
00132 
00133     direction = new_direction;
00134 
00135 #if DEBUG
00136     log_direction();
00137 #endif
00138     update_direction_characteristic();
00139 }
00140 
00141 void disconnectionCallback(void)
00142 {
00143     pc.printf("Disconnected!\n");
00144     pc.printf("Restarting the advertising process\n");
00145     ble.startAdvertising();
00146 }
00147 
00148 void periodicCallback(void)
00149 {
00150     update_cube_direction();
00151 }
00152 
00153 void setup_ble(void)
00154 {
00155     ble.init();
00156     ble.onDisconnection(disconnectionCallback);
00157 
00158     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00159     ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
00160                                      beaconPayload, sizeof(beaconPayload));
00161 
00162     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00163     ble.setAdvertisingInterval(160);
00164 
00165     ble.startAdvertising();
00166 
00167     ble.addService(cube_service);
00168 
00169     pc.printf("BLE set up and running\n");
00170 }
00171 
00172 int main()
00173 {
00174     setup_ble();
00175 
00176     Ticker ticker;
00177     ticker.attach(periodicCallback, 1);
00178 
00179     pc.printf("MPU6050 test startup:\n");
00180 
00181     mpu.initialize();
00182     pc.printf("TestConnection\n");
00183 
00184     if (mpu.testConnection()) {
00185         pc.printf("MPU success\n");
00186     } else {
00187         pc.printf("MPU error\n");
00188     }
00189 
00190     while(1) {
00191         ble.waitForEvent();
00192     }
00193 }