File content as of revision 5:47cb37923f58:

#include "TrainingCommand.h"

int16_t dummyAngle = 0;

DigitalOut blue(p15);
DigitalOut data(p13);

TrainingCommand::TrainingCommand(Sensors* sensors, GonioService* gonioService)
{
    this->sensors = sensors;
    this->gonioService = gonioService;
    blue = 0;
    data = 0;

}

void TrainingCommand::initialize()
{
    dummyAngle = 1;
    setTicker(0.1);
}


void TrainingCommand::execute()
{
    /*
    int8_t peak;
    int8_t readInt = sensors->getInterruptStatus();
    int8_t statusInt = sensors->getMotionDetectionStatus();

    if((((readInt >> 5) & 0x1)+ ((readInt >> 6) & 0x1)+ ((readInt >> 7))&0x1) != 0) {
        peak = 1;
        if((readInt >> 5) & 0x1) {
            //printf(" ZERO MOTION ");
        }
        if((readInt >> 6) & 0x1) {
            //printf(" MOTION DETECTION ");
        }
        if((readInt >> 7) & 0x1) {
            //printf(" FREE FALL ");
        }
        //printf("status: %d\n", statusInt);
    }

    if (readInt & 0x01) {
        //sensors->updateAngle();
        //int16_t angle = (sensors->getAngle(0) << 8) + sensors->getAngle(1);
        //printf("%d,%d\n", dummyAngle, peak);
        gonioService->updateGonio(dummyAngle,0xffff,peak);
        dummyAngle++;
        peak = 0;
    }
    */

    data = 1;
    sensors->updateAngle();
    sensors->updateIMU();

    int16_t angle = (sensors->getAngle(0) << 8) + sensors->getAngle(1);
    //int16_t angle = (sensors->getAngleDummy(0) << 8) + sensors->getAngleDummy(1);
    //__disable_irq();
    data = 0;
    
    wait(0.001);

    if(gonioService->isConnected()) {
        blue = 1;
               gonioService->updateGonio(angle,angle,angle);
        blue = 0;
    }

    //__enable_irq();
}

void TrainingCommand::button()
{
    detachTicker();
    setLed(1);
    sensors->wakeIMU();
    printf("****   BUTTON: TrainingCommand    ****\n");
}

void TrainingCommand::finish()
{
    sensors->disableIMU();
    setLed(0);
}

void TrainingCommand::printSensors()
{
    sensors->updateIMU();

    int16_t accel[3] = {0,0,0};
    int16_t gyro[3] = {0,0,0};

    accel[0] = (sensors->getIMU(0)<<8)+sensors->getIMU(1);
    accel[1] = (sensors->getIMU(2)<<8)+sensors->getIMU(3);
    accel[2] = (sensors->getIMU(4)<<8)+sensors->getIMU(5);
    gyro[0]  = (sensors->getIMU(6)<<8)+sensors->getIMU(7);
    gyro[1]  = (sensors->getIMU(8)<<8)+sensors->getIMU(9);
    gyro[2]  = (sensors->getIMU(10)<<8)+sensors->getIMU(11);

    float xangle = 57.295*atan((float)accel[1]/ sqrt(pow((float)accel[2],2)+pow((float)accel[0],2)));
    float yangle = 57.295*atan((float)-accel[0]/ sqrt(pow((float)accel[2],2)+pow((float)accel[1],2)));

    float xgyro = ((float)gyro[0])/16.4;

    printf("Aceleration Angles: %f, %f \n", xangle, yangle);
    printf("gyroscope x: %f\n",xgyro);

}