Diff: main.cpp

Revision:

1:5bf45482441d

Parent:

0:31128b41490c

--- a/main.cpp	Mon Oct 19 04:20:24 2015 +0000
+++ b/main.cpp	Tue Oct 20 04:23:32 2015 +0000
@@ -51,6 +51,7 @@
 int initialHeading = 0;
 int state = 0;
 int difference = 0;
+int counter = 0;
 void setup()
 {
     uLCD.cls();
@@ -114,15 +115,16 @@
 {
     imu.readMag();
     myLED3 = 1;
-    uLCD.locate(1,5);
-    uLCD.printf("Turning Right \n");
-    mA.speed(0.2);
-    mB.speed(-0.2);
+//    uLCD.locate(1,5);
+//    uLCD.printf("Turning Right \n");
+    mA.speed(0.25);
+    mB.speed(-0.25);
     heading = static_cast<int> (imu.calcHeading());
-    uLCD.printf("H : %d \n",heading);
+//    uLCD.printf("H : %d \n",heading);
     degree =   heading - initialHeading;
     degree = turningDegree - degree;
-    uLCD.printf("Degree = %d \n",degree);
+    uLCD.locate(1,5);
+    uLCD.printf("Degree Left = %d \n",degree);
     uLCD.printf("Heading = %d \n",heading);
 }
 
@@ -130,15 +132,16 @@
 {
     imu.readMag();
     myLED3 = 1;
-    uLCD.locate(1,5);
-    uLCD.printf("Turning Left \n");
-    mA.speed(-0.2);
-    mB.speed(0.2);
+//    uLCD.locate(1,5);
+//    uLCD.printf("Turning Left \n");
+    mA.speed(-0.25);
+    mB.speed(0.25);
     heading = static_cast<int> (imu.calcHeading());
-    uLCD.printf("H : %d \n",heading);
+//    uLCD.printf("H : %d \n",heading);
     degree =   heading - initialHeading;
     degree = turningDegree - degree;
-    uLCD.printf("Degree = %d \n",degree);
+    uLCD.locate(1,5);
+    uLCD.printf("Degree Left = %d \n",degree);
     uLCD.printf("Heading = %d \n",heading);
 }
 
@@ -172,14 +175,14 @@
         //Setting mode
         while(settingMode == 0)
         {
-            uLCD.locate(1,2);
+            uLCD.locate(1,1);
             if (mode1 == SETX)
             {
-                uLCD.printf("Setting X coordinate");    
+                uLCD.printf("Setting X Coord");    
             }
             else if (mode1 == SETY)
             {
-                uLCD.printf("Setting Y Coordinate");   
+                uLCD.printf("Setting Y Coord");   
             }
             else if (mode1 == GO)
             {
@@ -200,9 +203,9 @@
                 initialHeading = static_cast<int>(imu.calcHeading());
                 angle = atan2 (static_cast<float> (yCoord),static_cast<float>(xCoord)) * 180 / PI;
                 finalDegree = initialHeading + angle;
-                uLCD.locate(4,9);
+                uLCD.locate(1,9);
                 uLCD.printf("Distance: %5.2F Feet \n\r", distance);
-                uLCD.locate(4,11);
+                uLCD.locate(1,11);
                 uLCD.printf("Turning: %5.2F Degree \n\r", angle);
                 
                 settingMode = 1;
@@ -214,7 +217,7 @@
         while(settingMode == 1)
         {
             uLCD.cls();
-            uLCD.printf("Setting mode 1");
+//            uLCD.printf("Setting mode 1");
             imu.readMag();
             initialHeading = static_cast<int> (imu.calcHeading());
             uLCD.locate(1,3);
@@ -229,9 +232,10 @@
                 while (degree  > -2 && degree < 180)
                 {              
                     turnRight();
-                    wait(0.2);
+                    
                     mA.speed(0);
                     mB.speed(0);  
+                    wait(0.4);
                 } 
                 settingMode = 2;
 
@@ -241,9 +245,10 @@
                 while (degree < 2 && degree > -180 )
                 {
                     turnLeft();
-                    wait(0.2);
+                    
                     mA.speed(0);
-                    mB.speed(0);  
+                    mB.speed(0);
+                    wait(0.4);  
                 }
                 settingMode = 2;
             }                                                  
@@ -253,28 +258,37 @@
             imu.readMag();
             initialHeading = static_cast<int> (imu.calcHeading());
             timer.start();
+            uLCD.cls();
             while(timer.read_ms() < static_cast<int>(seconds) && settingMode == 2)
             {
-                uLCD.locate(1,7);
-                uLCD.printf("Time : %d \n",timer.read_ms());
+            
+                
+//                uLCD.printf("Time : %d \n",timer.read_ms());
                 mA.speed(0.4);
                 mB.speed(0.4);
                 imu.readMag();
                 heading = static_cast<int> (imu.calcHeading());
-                difference = heading - initialHeading;
-                uLCD.locate(1,3);
-                uLCD.printf("Heading : %d \n",heading);
-                if( difference <-2)
+                difference = heading - finalDegree;
+                uLCD.locate(5,6);
+                uLCD.printf("Go!");
+                while( difference <-2 &&timer.read_ms() < static_cast<int>(seconds) )
                 {
+                            
                             mA.speed(0.5);
                             mB.speed(0.4);
                             imu.readMag();
                             heading = static_cast<int> (imu.calcHeading());
                             difference = heading - finalDegree;
                             uLCD.locate(1,3);
-                            uLCD.printf("Heading : %d \n",heading);
+                            uLCD.printf("Difference : %d \n",difference);
+                            uLCD.locate(1,4);
+                            uLCD.printf("Turning Right \n");
+                            uLCD.locate(1,5);
+                            uLCD.printf("Counter = %d \n",counter);
+                            counter++;
+//                            wait(0.2);
                 }
-                 if(difference >2)
+                 while(difference >2 && timer.read_ms() < static_cast<int>(seconds))
                 {
                             mA.speed(0.4);
                             mB.speed(0.5);
@@ -282,7 +296,13 @@
                             heading = static_cast<int> (imu.calcHeading());
                             difference = heading - finalDegree;
                             uLCD.locate(1,3);
-                            uLCD.printf("Heading : %d \n",heading);
+                            uLCD.printf("Difference : %d \n",difference);
+                            uLCD.locate(1,4);
+                            uLCD.printf("Turning Left \n");
+                            uLCD.locate(1,5);
+                            uLCD.printf("Counter = %d \n",counter);
+                            counter++;
+//                            wait(0.2);
                 }                                    
             }
             mA.speed(0);