Diff: TrainingCommand.cpp

Revision:

0:e188325211af

Child:

1:fd4c0e2decb8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TrainingCommand.cpp	Thu Jun 04 10:56:18 2015 +0000
@@ -0,0 +1,39 @@
+#include "TrainingCommand.h"
+
+TrainingCommand::TrainingCommand(){
+    this->sensors = Sensors::getInstance();
+    }
+
+void TrainingCommand::initialize(){
+    printf("TrainingCommand\n");
+    }
+
+
+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\n",xgyro);
+    printf("temperature: %f\n\n",sensors->getTemp());
+    }
\ No newline at end of file