Diff: TrainingCommand.cpp

Revision:

5:47cb37923f58

Parent:

4:f81029197ab2

--- a/TrainingCommand.cpp	Thu Jun 11 10:42:50 2015 +0000
+++ b/TrainingCommand.cpp	Thu Jun 11 20:57:59 2015 +0000
@@ -2,14 +2,20 @@
 
 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);
 }
@@ -34,7 +40,7 @@
             //printf(" FREE FALL ");
         }
         //printf("status: %d\n", statusInt);
-    } 
+    }
 
     if (readInt & 0x01) {
         //sensors->updateAngle();
@@ -45,12 +51,25 @@
         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;
     
-    //sensors->updateAngle();
-    //sensors->updateIMU();
-    int16_t angle = (sensors->getAngleDummy(0) << 8) + sensors->getAngleDummy(1);
-    gonioService->updateGonio(angle,angle,angle);
-    dummyAngle++;
+    wait(0.001);
+
+    if(gonioService->isConnected()) {
+        blue = 1;
+               gonioService->updateGonio(angle,angle,angle);
+        blue = 0;
+    }
+
+    //__enable_irq();
 }
 
 void TrainingCommand::button()