main.cpp

00001 /**
00002  * Copyright 2014 Nordic Semiconductor
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *  http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License
00015  */
00016 
00017 #include "MPU6050.h"
00018 #include <math.h>
00019 
00020 #include "Puck.h"
00021 
00022 Puck* puck = &Puck::getPuck();
00023 
00024 const UUID CUBE_SERVICE_UUID = stringToUUID("bftj cube       ");
00025 const UUID DIRECTION_UUID = stringToUUID("bftj cube dirctn");
00026 
00027 enum Direction {
00028     UP,
00029     DOWN,
00030     LEFT,
00031     RIGHT,
00032     FRONT,
00033     BACK,
00034     UNDEFINED
00035 };
00036 
00037 const static int16_t ACCELERATION_EXITATION_THRESHOLD = 15000;
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(Direction direction) {
00047     switch(direction) {
00048         case UP: LOG_INFO("Direction UP\n"); break;
00049         case DOWN: LOG_INFO("Direction DOWN\n"); break;
00050         case LEFT: LOG_INFO("Direction LEFT\n"); break;
00051         case RIGHT: LOG_INFO("Direction RIGHT\n"); break;
00052         case BACK: LOG_INFO("Direction BACK\n"); break;
00053         case FRONT: LOG_INFO("Direction FRONT\n"); break;
00054         default: LOG_INFO("Direction UNSET\n"); break;
00055     }
00056 }
00057 
00058 int16_t direction_if_exited(int16_t acceleration) {
00059     if (acceleration > ACCELERATION_EXITATION_THRESHOLD) {
00060         return 1;
00061     }
00062     if (acceleration < -ACCELERATION_EXITATION_THRESHOLD) {
00063         return -1;
00064     }
00065     return 0;
00066 }
00067 
00068 void updateCubeDirection(void) {
00069     
00070     if(!mpu.testConnection()) {
00071         LOG_ERROR("MPU DIED! Resetting...\n");
00072         mpu.reset();
00073         mpu.initialize();
00074         LOG_ERROR("Reset complete.\n");
00075         return;
00076     }
00077     
00078     mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00079 
00080     int16_t x = direction_if_exited(ax);
00081     int16_t y = direction_if_exited(ay);
00082     int16_t z = direction_if_exited(az);
00083 
00084     int16_t sum = abs(x) + abs(y) + abs(z);
00085     if (sum != 1) {
00086         return;
00087     }
00088 
00089     Direction new_direction = UNDEFINED;
00090     if (z == 1) {
00091         new_direction = UP;
00092     } else if (z == -1) {
00093         new_direction = DOWN;
00094     } else if (y == 1) {
00095         new_direction = LEFT;
00096     } else if (y == -1) {
00097         new_direction = RIGHT;
00098     } else if (x == 1) {
00099         new_direction = BACK;
00100     } else if (x == -1) {
00101         new_direction = FRONT;
00102     }
00103 
00104     if (direction == new_direction) {
00105         return;
00106     }
00107 
00108     direction = new_direction;
00109 
00110     log_direction(direction);
00111     uint8_t directionAsInteger = direction;
00112     int length = 1;
00113     puck->updateCharacteristicValue(DIRECTION_UUID, &directionAsInteger, length);
00114 }
00115 
00116 
00117 int main() {
00118     
00119 
00120     LOG_VERBOSE("MPU6050 test startup:\n");
00121 
00122     mpu.initialize();
00123     LOG_VERBOSE("TestConnection\n");
00124 
00125     if (mpu.testConnection()) {
00126         LOG_INFO("MPU initialized.\n");
00127     } else {
00128         LOG_ERROR("MPU not properly initialized!\n");
00129     }
00130 
00131     int characteristicValueLength = 1;
00132     puck->addCharacteristic(
00133         CUBE_SERVICE_UUID,
00134         DIRECTION_UUID,
00135         characteristicValueLength,
00136         GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00137     
00138     puck->init(0xC1BE);
00139     
00140     
00141     Ticker ticker;
00142     ticker.attach(updateCubeDirection, 0.2);
00143     LOG_INFO("Started listening to orientation changes.\n");
00144 
00145     while(puck->drive());
00146 }