Diff: main.cpp

Revision:

13:f8e7af7ad87a

Parent:

12:dfa473dd9311

--- a/main.cpp	Mon May 02 05:19:35 2016 +0000
+++ b/main.cpp	Mon May 02 05:56:24 2016 +0000
@@ -109,19 +109,20 @@
        // 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] * .1;
         posY[0] += velY[0] * .1;  
         wait(.1);  
     }    
-}    
+}    */
 void left_thread(void const *args) {
     while (1) {
         leftDist = infraredL * 80.0f;  
@@ -171,8 +172,13 @@
     MotorRi.speed(0.4);
     MotorLe.speed(-0.4);
     Thread::wait(3000);         //allow mag to calibrate by spinning
+    MotorRi.speed(0.0);
+    MotorLe.speed(-0.0);
+    Thread::wait(10000);
     while(1) {
         if (fdist > 40) {                   //if there's room to go forward
+            posX[0] += cos(head) * 0.0001;
+            posY[0] += sin(head) * 0.0001;
             if (ldist < 20) {
                 MotorRi.speed(0.1);
                 MotorLe.speed(0.3);
@@ -191,12 +197,12 @@
         else if (rdist < ldist) {               //hard turn to the left
             MotorRi.speed(0.3);
             MotorLe.speed(-0.3); 
-            Thread::wait(100);         
+            Thread::wait(500);         
         }
         else if (ldist < rdist) {               //hard turn to the right
             MotorRi.speed(-0.4);
             MotorLe.speed(0.4);   
-            Thread::wait(100);        
+            Thread::wait(500);        
         }
     }
 }        
@@ -204,7 +210,7 @@
 int main() {
     //Test
     t.start();
-    Thread posi(pos_thread);
+    //Thread posi(pos_thread);
     Thread left(left_thread);
     Thread right(right_thread);
     Thread front(front_thread);