Diff: TrainingCommand.cpp

Revision:

4:f81029197ab2

Parent:

3:32afe87d4b62

Child:

5:47cb37923f58

--- a/TrainingCommand.cpp	Sun Jun 07 21:57:46 2015 +0000
+++ b/TrainingCommand.cpp	Thu Jun 11 10:42:50 2015 +0000
@@ -1,55 +1,62 @@
 #include "TrainingCommand.h"
 
-
+int16_t dummyAngle = 0;
 
-TrainingCommand::TrainingCommand(Sensors* sensors)
+TrainingCommand::TrainingCommand(Sensors* sensors, GonioService* gonioService)
 {
     this->sensors = sensors;
+    this->gonioService = gonioService;
 }
 
 void TrainingCommand::initialize()
-{
-    printf("TrainingCommand\n");
+{    
+    dummyAngle = 1;
     setTicker(0.1);
 }
 
 
 void TrainingCommand::execute()
 {
-    //IMU
-    printf("peak: ");
+    /*
+    int8_t peak;
     int8_t readInt = sensors->getInterruptStatus();
-    int8_t statusInt = sensors->readRegister(0x61);
-    
-    //[7] MOT_XNEG    [6] MOT_XPOS    [5] MOT_YNEG    [4] MOT_YPOS    [3] MOT_ZNEG    [2] MOT_ZPOS
-    
+    int8_t statusInt = sensors->getMotionDetectionStatus();
+
     if((((readInt >> 5) & 0x1)+ ((readInt >> 6) & 0x1)+ ((readInt >> 7))&0x1) != 0) {
-        printf("1 =>");
+        peak = 1;
         if((readInt >> 5) & 0x1) {
-            printf(" ZERO MOTION ");
+            //printf(" ZERO MOTION ");
         }
         if((readInt >> 6) & 0x1) {
-            printf(" MOTION DETECTION ");
+            //printf(" MOTION DETECTION ");
         }
         if((readInt >> 7) & 0x1) {
-            printf(" FREE FALL ");
+            //printf(" FREE FALL ");
         }
-        printf("status: %d\n", statusInt);
-    } else {printf("0\n");}
-    
-    //printSensors();
+        //printf("status: %d\n", statusInt);
+    } 
 
-    //Angle
-    sensors->updateAngle();
-    int16_t angle = (sensors->getAngle(0) << 8) + sensors->getAngle(1);
-    printf("angle: %.2f\n\n", ((float)angle) * 0.087912087);
-
+    if (readInt & 0x01) {
+        //sensors->updateAngle();
+        //int16_t angle = (sensors->getAngle(0) << 8) + sensors->getAngle(1);
+        //printf("%d,%d\n", dummyAngle, peak);
+        gonioService->updateGonio(dummyAngle,0xffff,peak);
+        dummyAngle++;
+        peak = 0;
+    }
+    */
     
+    //sensors->updateAngle();
+    //sensors->updateIMU();
+    int16_t angle = (sensors->getAngleDummy(0) << 8) + sensors->getAngleDummy(1);
+    gonioService->updateGonio(angle,angle,angle);
+    dummyAngle++;
 }
 
 void TrainingCommand::button()
 {
     detachTicker();
+    setLed(1);
     sensors->wakeIMU();
     printf("****   BUTTON: TrainingCommand    ****\n");
 }
@@ -66,7 +73,7 @@
 
     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);
@@ -81,5 +88,5 @@
 
     printf("Aceleration Angles: %f, %f \n", xangle, yangle);
     printf("gyroscope x: %f\n",xgyro);
-    
+
 }
\ No newline at end of file