Diff: main.cpp

Revision:

9:6d663bdfee7c

Parent:

8:c7d277d05f23

Child:

10:168a1b176c77

--- a/main.cpp	Wed Oct 10 06:48:16 2012 +0000
+++ b/main.cpp	Wed Oct 10 07:48:36 2012 +0000
@@ -33,14 +33,14 @@
     
     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;
-        for(j=0;j<4;j++) stable[3][j] = diagRight;
-        for(j=0;j<4;j++) stable[4][j] = readFront;
+        for(j=0;j<2;j++) stable[0][j] = readLeft;
+        for(j=0;j<2;j++) stable[1][j] = readRight;
+        for(j=0;j<2;j++) stable[2][j] = diagLeft;
+        for(j=0;j<2;j++) stable[3][j] = diagRight;
+        for(j=0;j<2;j++) stable[4][j] = readFront;
         j=1;
     while (1){
-        if(j>=4) j=0;
+        if(j>=2) j=0;
        
         readLeft=irLeft;
         readRight=irRight;
@@ -69,18 +69,11 @@
 
         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(stable[0][0]<35||stable[0][1]<35)||stable[1][0]<35||stable[1][1]<35); NorthOn=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)
-                    /*(avgLeft<40&&stable[0][0]<40&&stable[0][1]<40&&stable[0][2]<40&&stable[0][3]<40)*/)
+                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)*/
+                    /*(avgLeft<40&&stable[0][0]<40&&stable[0][1]<40&&stable[0][2]<40&&stable[0][3]<40)*/))
                 {
                     right.speed(0);
                     left.speed(0);
@@ -89,8 +82,8 @@
                     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<35&&stable[3][0]<35&&stable[3][1]<35/*&&stable[3][2]<35&&stable[3][3]<35)*/
+                        /*(avgRight<40&&stable[1][0]<40&&stable[1][1]<40&&stable[1][2]<40&&stable[1][3]<40))*/))
                 {
                     right.speed(0);
                     left.speed(0);
@@ -115,8 +108,8 @@
             case Right:
                 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))
+                   (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)*/
                 )
                 {
                     right.speed(-1);
@@ -124,11 +117,7 @@
                    // 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");
@@ -141,8 +130,8 @@
                 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)
-                || (diagRight<=31.7||stable[3][0]<30||stable[3][1]<30||stable[3][2]<30||stable[3][3]<30)
+                   (avgFront<=35||(stable[4][0]<35||stable[4][1]<35))/*&&(stable[4][2]<35||stable[4][3]<35))*/
+                    ||(diagRight<=35||(stable[3][0]<35||stable[3][1]<35))/*&&(stable[3][2]<35||stable[3][3]<35))*/
                    )
                 {
                     right.speed(0);
@@ -151,11 +140,7 @@
                 }
                 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");