Diff: main.cpp

Revision:

0:449ee9595cf6

Child:

2:b9b42ff80e9a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 03 11:25:12 2014 +0000
@@ -0,0 +1,182 @@
+#include "mbed.h"
+#include "BLEDevice.h"
+#include "nRF51822n.h"
+#include "MPU6050.h"
+#include <math.h> 
+
+#define DEBUG 1
+
+enum Direction {
+    UP,
+    DOWN,
+    LEFT,
+    RIGHT,
+    FRONT,
+    BACK,
+    UNDEFINED
+};
+
+Serial pc(USBTX, USBRX);
+
+BLEDevice ble;
+
+const static int16_t ACCELERATION_EXITATION_THRESHOLD = 15000;
+const static uint8_t beaconPayload[] = {
+    0x00, 0x4C, // Company identifier code (0x004C == Apple)
+    0x02,       // ID
+    0x15,       // length of the remaining payload
+    0xE2, 0x0A, 0x39, 0xF4, 0x73, 0xF5, 0x4B, 0xC4, // UUID
+    0xA1, 0x2F, 0x17, 0xD1, 0xAD, 0x07, 0xA9, 0x61,
+    0x13, 0x37, // the major value to differenciate a location
+    0xFA, 0xCE, // the minor value to differenciate a location
+    0xC8        // 2's complement of the Tx power (-56dB)
+};
+
+extern GattService cube_service;
+extern GattCharacteristic directionCharacteristic;
+extern uint8_t direction_data[1];
+
+MPU6050 mpu;
+
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+
+Direction direction = UNDEFINED;
+
+void log_direction(void)
+{
+    switch(direction)
+    {
+        case UP:
+            pc.printf("Direction UP\n");
+            break;
+            
+        case DOWN:
+            pc.printf("Direction DOWN\n");
+            break;
+            
+        case LEFT:
+            pc.printf("Direction LEFT\n");
+            break;
+            
+        case RIGHT:
+            pc.printf("Direction RIGHT\n");
+            break;
+            
+        case BACK:
+            pc.printf("Direction BACK\n");
+            break;
+            
+        case FRONT:
+            pc.printf("Direction FRONT\n");
+            break;
+            
+        default:
+            pc.printf("Direction UNSET\n");
+            break;
+    }
+}
+
+int16_t inline direction_if_exited(int16_t acceleration) {
+    if (acceleration > ACCELERATION_EXITATION_THRESHOLD) {
+        return 1;
+    } else if (acceleration < -ACCELERATION_EXITATION_THRESHOLD) {
+        return -1;
+    } else {
+        return 0;
+    }
+}
+
+void update_cube_direction(void)
+{
+    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+    
+    int16_t x = direction_if_exited(ax);
+    int16_t y = direction_if_exited(ay);
+    int16_t z = direction_if_exited(az);
+    
+    int16_t sum = abs(x) + abs(y) + abs(z);
+    if (sum == 0 || sum != 1) {
+        return;
+    }
+    
+    if (z == 1) {
+        direction = UP;
+    } else if (z == -1) {
+        direction = DOWN;
+    } else if (y == 1) {
+        direction = LEFT;
+    } else if (y == -1) {
+        direction = RIGHT;
+    } else if (x == 1) {
+        direction = BACK;
+    } else if (x == -1) {
+        direction = FRONT;
+    }
+    
+#if DEBUG
+    log_direction();
+#endif
+}
+
+void update_direction_characteristic(void)
+{    
+    direction_data[0] = direction;
+    ble.updateCharacteristicValue(directionCharacteristic.getHandle(),
+            direction_data,
+            sizeof(direction_data));
+#if DEBUG
+    pc.printf("Updated gatt characteristic\n");
+#endif
+}
+
+void disconnectionCallback(void)
+{
+    pc.printf("Disconnected!\n");
+    pc.printf("Restarting the advertising process\n");
+    ble.startAdvertising();
+}
+
+void setup_ble(void)
+{
+    ble.init();
+    ble.onDisconnection(disconnectionCallback);
+
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA,
+                    beaconPayload, sizeof(beaconPayload));
+                    
+    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    ble.setAdvertisingInterval(160);
+
+    ble.startAdvertising();
+    
+    ble.addService(cube_service);
+    
+    pc.printf("BLE set up and running\n");
+}
+
+int main()
+{
+    setup_ble();
+    pc.printf("MPU6050 test startup:\n");
+
+    mpu.initialize();
+    pc.printf("TestConnection\n");
+    
+    if (mpu.testConnection())
+    {
+        pc.printf("MPU success\n");
+    }
+    else
+    {
+        pc.printf("MPU error\n");
+    }
+
+    while(1)
+    {
+        ble.waitForEvent();
+        update_cube_direction();
+        update_direction_characteristic();
+    }
+}
\ No newline at end of file