Diff: main.cpp

Revision:

11:805ef606a29d

Parent:

9:36807f7e58fb

Child:

12:dfa473dd9311

--- a/main.cpp	Mon May 02 04:36:24 2016 +0000
+++ b/main.cpp	Mon May 02 04:39:20 2016 +0000
@@ -105,17 +105,21 @@
         ymag = IMU.calcMag(IMU.my);
         zmag = IMU.calcMag(IMU.mz);
         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        
+       // Thread::wait(50); //Wait 
+    }    
+}
+void pos_thread(void const *args) {
+    while(1) {
         shiftArrays();
         imu[0] = xaccel;
         compass[0] = head;
         velX[0] += imu[0]*cos(compass[0])  * deltat;
         velY[0] += imu[0]*sin(compass[0])  * deltat;
         posX[0] += velX[0] * deltat;
-        posY[0] += velY[0] * deltat;
-       // Thread::wait(50); //Wait 
+        posY[0] += velY[0] * deltat;    
     }    
-}
-
+}    
 void left_thread(void const *args) {
     while (1) {
         leftDist = infraredL * 80.0f;  
@@ -197,6 +201,7 @@
 int main() {
     //Test
     t.start();
+    Thread posi(pos_thread);
     Thread left(left_thread);
     Thread right(right_thread);
     Thread front(front_thread);