Diff: main.cpp

Revision:

11:0fd03f66af0d

Parent:

10:168a1b176c77

Child:

12:204600e080c4

--- a/main.cpp	Wed Oct 10 08:22:18 2012 +0000
+++ b/main.cpp	Wed Oct 10 08:27:10 2012 +0000
@@ -17,7 +17,7 @@
     enum direction dir=Forward;
     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     
-    printf("Front Right Left\n\r");
+    //printf("Front Right Left\n\r");
     int j=0;
     float readLeft;
     float readRight;
@@ -63,9 +63,9 @@
 stable[4][j] = avgFront;
 
 j++;
-        printf("Front: %5f Right: %5f Left: %5f dLeft: %5f dRight: %5f", avgFront, avgRight, avgLeft,diagLeft,diagRight);
+        //printf("Front: %5f Right: %5f Left: %5f dLeft: %5f dRight: %5f", avgFront, avgRight, avgLeft,diagLeft,diagRight);
         float globalDir = compass.sample()/10.0;
-        printf(" Heading: %f\n\r", globalDir);
+        //printf(" Heading: %f\n\r", globalDir);
 
         switch(dir){
             case Forward:
@@ -75,7 +75,7 @@
                 {
                     right.speed(0);
                     left.speed(0);
-                    printf("stopped!\n\r");
+                    //printf("stopped!\n\r");
                     //wait(0.1);
                     NorthOn=0;
                     dir=Right;
@@ -85,7 +85,7 @@
                 {
                     right.speed(0);
                     left.speed(0);
-                    printf("stopped2!\n\r");
+                    //printf("stopped2!\n\r");
                     //wait(0.1);
                     NorthOn=0;
                     dir=Left;
@@ -97,14 +97,14 @@
                             left.speed(0);
                             dir = TurnNorth;
                 }
-                else {right.speed(0.95); left.speed(1);
-                printf("CHARGE!\n\r");
-                if((avgLeft<35&&stable[0][0]<35&&stable[0][1]<35)
-                   &&(avgRight<35&&stable[1][0]<35&&stable[1][1]<35)) NorthOn=1;
+                else {right.speed(1); left.speed(1);
+                //printf("CHARGE!\n\r");
+                if((avgLeft>35&&stable[0][0]>35&&stable[0][1]>35)
+                   &&(avgRight>35&&stable[1][0]>35&&stable[1][1]>35)) NorthOn=1;
                 }
             break;
             case Right:
-                printf("TURN Right!\n\r");
+                ////printf("TURN Right!\n\r");
                 if (
                    (avgFront<=35||stable[4][0]<35||stable[4][1]<35)/*||stable[4][2]<35||stable[4][3]<35)*/
                 || (diagLeft<=35||stable[2][0]<35||stable[2][1]<35)/*||stable[2][2]<35||stable[2][3]<35)*/
@@ -112,20 +112,20 @@
                 {
                     right.speed(-1);
                     left.speed(0);
-                   // printf("turning!\n\r");
+                   // //printf("turning!\n\r");
                 }
                 else 
                 {
                     right.speed(0);
                     left.speed(0);
-                  //  printf("stopped!\n\r");
+                  //  //printf("stopped!\n\r");
                     //wait(0.1);
                     dir=Forward;
                 }
             break;
             
             case Left:
-                printf("TURN Left!\n\r");
+                //printf("TURN Left!\n\r");
                 if (
                    /*(avgRight<=31.7 ||stable[1][0]<30||stable[1][1]<30||stable[1][2]<30||stable[1][3]<30)||*/
                    (avgFront<=35||(stable[4][0]<35||stable[4][1]<35))/*&&(stable[4][2]<35||stable[4][3]<35))*/
@@ -134,20 +134,20 @@
                 {
                     right.speed(0);
                     left.speed(-1);
-                   // printf("turning!\n\r");
+                   // //printf("turning!\n\r");
                 }
                 else 
                 {
                 
                     right.speed(0);
                     left.speed(0);
-                  //  printf("stopped!\n\r");
+                  //  //printf("stopped!\n\r");
                     //wait(0.1);
                     dir=Forward;
                 }
             break;
             case TurnNorth:
-                printf("TO THE NORTH!\n\r");
+                //printf("TO THE NORTH!\n\r");
                 if(
                    (avgLeft<35||stable[0][0]<35||stable[0][1]<35)
                    ||
@@ -158,7 +158,10 @@
                    (diagRight<35||stable[3][0]<35||stable[3][1]<35)
                    ||
                    (avgFront<35||stable[4][0]<35||stable[4][1]<35)
-                              
+                   )
+                   {right.speed(0);
+                    left.speed(0);
+                    dir=Forward;}    
                 if (globalDir>330 || globalDir < 30)
                 {
                     //It's going north!!