Diff: main.cpp

Revision:

8:c7d277d05f23

Parent:

7:60e5e064698d

Child:

9:6d663bdfee7c

--- a/main.cpp	Wed Oct 10 06:28:46 2012 +0000
+++ b/main.cpp	Wed Oct 10 06:48:16 2012 +0000
@@ -32,6 +32,7 @@
         diagLeft=dirLeft;
     
     int NorthOn=0;
+    int NorthCount=0;
         for(j=0;j<4;j++) stable[0][j] = readLeft;
         for(j=0;j<4;j++) stable[1][j] = readRight;
         for(j=0;j<4;j++) stable[2][j] = diagLeft;
@@ -68,27 +69,38 @@
 
         switch(dir){
             case Forward:
+                if(
+                ((stable[0][0]+stable[0][1]+stable[0][2]+stable[0][3])>=160)&&
+                ((stable[1][0]+stable[1][1]+stable[1][2]+stable[1][3])>=160)&&
+                ((stable[2][0]+stable[2][1]+stable[2][2]+stable[2][3])>=160)&&
+                ((stable[3][0]+stable[3][1]+stable[3][2]+stable[3][3])>=160)&&
+                ((stable[4][0]+stable[4][1]+stable[4][2]+stable[4][3])>=160)) NorthCount++;
+                else NorthCount=0;
+                if (NorthCount>3){ NorthOn=1; NorthCount=0;}
+                
                 if ((avgFront<40&&stable[4][0]<40&&stable[4][1]<40&&stable[4][2]<40&&stable[4][3]<40)||
-                    (diagLeft<40&&stable[2][0]<40&&stable[2][1]<40&&stable[2][2]<40&&stable[2][3]<40)||
+                    (diagLeft<40&&stable[2][0]<40&&stable[2][1]<40&&stable[2][2]<40&&stable[2][3]<40)
                     /*(avgLeft<40&&stable[0][0]<40&&stable[0][1]<40&&stable[0][2]<40&&stable[0][3]<40)*/)
                 {
                     right.speed(0);
                     left.speed(0);
                     printf("stopped!\n\r");
                     //wait(0.1);
+                    NorthOn=0;
                     dir=Right;
                 }
-                else if((diagRight<40&&stable[3][0]<40&&stable[3][1]<40&&stable[3][2]<40&&stable[3][3]<40)||
-                        /*(avgRight<40&&stable[1][0]<40&&stable[1][1]<40&&stable[1][2]<40&&stable[1][3]<40))*/
+                else if((diagRight<40&&stable[3][0]<40&&stable[3][1]<40&&stable[3][2]<40&&stable[3][3]<40)
+                        /*(avgRight<40&&stable[1][0]<40&&stable[1][1]<40&&stable[1][2]<40&&stable[1][3]<40))*/)
                 {
                     right.speed(0);
                     left.speed(0);
                     printf("stopped2!\n\r");
                     //wait(0.1);
+                    NorthOn=0;
                     dir=Left;
                 }
                 else if(
-                (((globalDir > 30 && globalDir <= 180)||(globalDir > 180 && globalDir <= 330))&&
+                (((globalDir > 30 && globalDir <= 180)||(globalDir > 180 && globalDir <= 330)&&NorthOn)
          )
                 )
                 {
@@ -104,15 +116,19 @@
                 printf("TURN Right!\n\r");
                 if (
                    ((avgFront<=31.7||stable[4][0]<30||stable[4][1]<30||stable[4][2]<30||stable[4][3]<30)
-                || (diagLeft<=31.7||stable[2][0]<30||stable[2][1]<30||stable[2][2]<30||stable[2][3]<30)
+                || (diagLeft<=31.7||stable[2][0]<30||stable[2][1]<30||stable[2][2]<30||stable[2][3]<30))
                 )
                 {
                     right.speed(-1);
-                    left.speed(1);
+                    left.speed(0);
                    // printf("turning!\n\r");
                 }
                 else 
-                {
+                { for(j=0;j<4;j++) stable[0][j] = avgLeft;
+                  for(j=0;j<4;j++) stable[1][j] = avgRight;
+                  for(j=0;j<4;j++) stable[2][j] = diagLeft;
+                  for(j=0;j<4;j++) stable[3][j] = diagRight;
+                  for(j=0;j<4;j++) stable[4][j] = avgFront;
                     right.speed(0);
                     left.speed(0);
                   //  printf("stopped!\n\r");
@@ -125,16 +141,21 @@
                 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<=31.7||stable[4][0]<30||stable[4][1]<30||stable[4][2]<30||stable[4][3]<30)
+                   (avgFront<=31.7||stable[4][0]<30||stable[4][1]<30||stable[4][2]<30||stable[4][3]<30)
                 || (diagRight<=31.7||stable[3][0]<30||stable[3][1]<30||stable[3][2]<30||stable[3][3]<30)
                    )
                 {
-                    right.speed(1);
+                    right.speed(0);
                     left.speed(-1);
                    // printf("turning!\n\r");
                 }
                 else 
                 {
+                for(j=0;j<4;j++) stable[0][j] = avgLeft;
+                  for(j=0;j<4;j++) stable[1][j] = avgRight;
+                  for(j=0;j<4;j++) stable[2][j] = diagLeft;
+                  for(j=0;j<4;j++) stable[3][j] = diagRight;
+                  for(j=0;j<4;j++) stable[4][j] = avgFront;
                     right.speed(0);
                     left.speed(0);
                   //  printf("stopped!\n\r");