Diff: IdleCommand.cpp

Revision:

4:f81029197ab2

Parent:

3:32afe87d4b62

Child:

5:47cb37923f58

--- a/IdleCommand.cpp	Sun Jun 07 21:57:46 2015 +0000
+++ b/IdleCommand.cpp	Thu Jun 11 10:42:50 2015 +0000
@@ -1,20 +1,43 @@
 #include "IdleCommand.h"
-
-IdleCommand::IdleCommand(Sensors* sensors) {
+ 
+IdleCommand::IdleCommand(Sensors* sensors, GonioService* gonioService) {
    this->sensors = sensors;
+   this->gonioService = gonioService;
     }
-
+ 
 void IdleCommand::initialize(){
     
     printf("IdleCommand\n");
     sensors->disableIMU();
     setTicker(0.5);
     }
-
+ 
 void IdleCommand::execute(){}
     
 void IdleCommand::button(){
     printf("****   BUTTON: IdleCommand   *****\n");
+       int16_t angle = (sensors->getAngle(0) << 8) + sensors->getAngle(1);
+    
+    //printf("%.2f,%d\n", ((float)angle) * 0.087912087, angle);
+    
+    sensors->updateAngle();
+    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);
+    
+    
+    if(gonioService->isConnected()){
+        gonioService->updateGonio(angle,gyro[0],accel[0]);
+    }
     }
     
 void IdleCommand::finish(){
@@ -23,3 +46,6 @@
     }
     
 
+
+
+