File content as of revision 2:c9e47ac47edb:

#include "TrainingCommand.h"



TrainingCommand::TrainingCommand(Sensors* sensors)
{
    this->sensors = sensors;
}

void TrainingCommand::initialize()
{
    printf("TrainingCommand\n");
    setLed(1);
}


void TrainingCommand::execute()
{
    printSensors();
}

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

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

void TrainingCommand::printSensors()
{
    sensors->updateIMUData();
    sensors->updateAngle();

    int8_t accel[3] = {0,0,0};
    int8_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])/200;

    printf("angle: %.2f\n", ((float)(sensors->getAngle(0) + sensors->getAngle(1))) * 0.087912087);
    printf("Aceleration Angles: %f, %f \n", xangle, yangle);

    printf("gyroscope x: %f\n",xgyro);
}