File content as of revision 8:4244572015c6:

/**
 * Copyright 2014 Nordic Semiconductor
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License
 */

#include "MPU6050.h"
#include <math.h>

#include "Puck.h"

Puck* puck = &Puck::getPuck();

const UUID CUBE_SERVICE_UUID = UUID(stringToUUID("bftj cube       "));
const UUID DIRECTION_UUID = UUID(stringToUUID("bftj cube dirctn"));

enum Direction {
    UP,
    DOWN,
    LEFT,
    RIGHT,
    FRONT,
    BACK,
    UNDEFINED
};

const static int16_t ACCELERATION_EXITATION_THRESHOLD = 15000;

MPU6050 mpu;

int16_t ax, ay, az;
int16_t gx, gy, gz;

Direction direction = UNDEFINED;

void log_direction(Direction direction) {
    switch(direction) {
        case UP: LOG_INFO("Direction UP\n"); break;
        case DOWN: LOG_INFO("Direction DOWN\n"); break;
        case LEFT: LOG_INFO("Direction LEFT\n"); break;
        case RIGHT: LOG_INFO("Direction RIGHT\n"); break;
        case BACK: LOG_INFO("Direction BACK\n"); break;
        case FRONT: LOG_INFO("Direction FRONT\n"); break;
        default: LOG_INFO("Direction UNSET\n"); break;
    }
}

int16_t direction_if_exited(int16_t acceleration) {
    if (acceleration > ACCELERATION_EXITATION_THRESHOLD) {
        return 1;
    }
    if (acceleration < -ACCELERATION_EXITATION_THRESHOLD) {
        return -1;
    }
    return 0;
}

void updateCubeDirection(void) {
    
    if(!mpu.testConnection()) {
        LOG_ERROR("MPU DIED! Resetting...\n");
        mpu.reset();
        mpu.initialize();
        LOG_ERROR("Reset complete.\n");
        return;
    }
    
    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 != 1) {
        return;
    }

    Direction new_direction = UNDEFINED;
    if (z == 1) {
        new_direction = UP;
    } else if (z == -1) {
        new_direction = DOWN;
    } else if (y == 1) {
        new_direction = LEFT;
    } else if (y == -1) {
        new_direction = RIGHT;
    } else if (x == 1) {
        new_direction = BACK;
    } else if (x == -1) {
        new_direction = FRONT;
    }

    if (direction == new_direction) {
        return;
    }

    direction = new_direction;

    log_direction(direction);
    uint8_t directionAsInteger = direction;
    int length = 1;
    puck->updateCharacteristicValue(DIRECTION_UUID, &directionAsInteger, length);
}


int main() {
    

    LOG_VERBOSE("MPU6050 test startup:\n");

    mpu.initialize();
    LOG_VERBOSE("TestConnection\n");

    if (mpu.testConnection()) {
        LOG_INFO("MPU initialized.\n");
    } else {
        LOG_ERROR("MPU not properly initialized!\n");
    }

    int characteristicValueLength = 1;
    puck->addCharacteristic(
        CUBE_SERVICE_UUID,
        DIRECTION_UUID,
        characteristicValueLength,
        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
    
    puck->init(0xC1BE);
    
    
    Ticker ticker;
    ticker.attach(updateCubeDirection, 0.2);
    LOG_INFO("Started listening to orientation changes.\n");

    while(puck->drive());
}