File content as of revision 1:fd4c0e2decb8:

#include "TrainingCommand.h"



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

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


void TrainingCommand::execute(){ 
    this->sensors->updateIMU();
    this->sensors->updateAngle();
    
    printSensors();
    }
    
void TrainingCommand::button(){
    this->sensors->wakeIMU();
    printf("****   BUTTON: TrainingCommand    ****\n");
    }
    
void TrainingCommand::finish(){
    setLed(0);
    this->sensors->disableIMU();
    }
    
void TrainingCommand::printSensors(){
    
    float xangle = 57.295*atan((float)sensors->getAccelY()/ sqrt(pow((float)sensors->getAccelZ(),2)+pow((float)sensors->getAccelX(),2)));
    float yangle = 57.295*atan((float)-sensors->getAccelX()/ sqrt(pow((float)sensors->getAccelZ(),2)+pow((float)sensors->getAccelY(),2)));
    float xgyro = ((float)sensors->getGyroX())/200;
    
    printf("angle: %.2f\n", sensors->getAngle());
    printf("Aceleration Angles: %f, %f \n", xangle, yangle);
     
    printf("gyroscope x: %f | ",xgyro);
    printf("temperature: %.2f\n\n",sensors->getTemp());
    }