Diff: main.cpp

Revision:

6:2b89c05628ce

Parent:

5:99545cf4dff4

Child:

7:60e5e064698d

--- a/main.cpp	Thu Oct 04 22:05:37 2012 +0000
+++ b/main.cpp	Thu Oct 04 22:53:42 2012 +0000
@@ -9,7 +9,7 @@
 AnalogIn irRight(p18);
 HMC6352 compass(p9, p10);
 
-enum direction{ Right, Forward };
+enum direction{ Right, Forward, Left, TurnNorth };
 
 int main() {
     enum direction dir=Forward;
@@ -52,7 +52,6 @@
         readFront=21/readFront;
         
         
-        printf("Heading: %f\n\r", compass.sample()/10.0);
         
        
         
@@ -75,27 +74,70 @@
         }
         avgFront = readFront;
         avgRight = readRight;
-        //printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft);
+        printf("Front: %5f Right: %5f Left: %5f", avgFront, avgRight, avgLeft);
+        float globalDir = compass.sample()/10.0;
+        printf(" Heading: %f\n\r", globalDir);
         
         switch(dir){
             case Forward:
+                printf("CHARGE!n\r");
                 if (avgFront <= 40)
                 {
                     right.speed(0);
                     left.speed(0);
-                   // printf("stopped!\n\r");
+                    printf("stopped!\n\r");
                     //wait(0.1);
                     dir=Right;
-                   // printf("less than 18\n\r");
                 }
                 else
                 {
-                    right.speed(1);
-                    left.speed(1);
+                    if (globalDir > 45 && globalDir <= 135)
+                    {
+                        //East
+                        if (avgLeft >= 50)
+                        {
+                            right.speed(0);
+                            left.speed(0);
+                            dir = TurnNorth;
+                        }
+                        else
+                        {
+                            right.speed(1);
+                            left.speed(1);
+                        }
+                    }
+                    else if (globalDir > 135 && globalDir <= 225)
+                    {
+                    //It's going south O.o
+                        right.speed(1);
+                        left.speed(1);
+                    }
+                    else if (globalDir > 225 && globalDir <= 315)
+                    {
+                        //West
+                        if (avgRight >= 50)
+                        {
+                            right.speed(0);
+                            left.speed(0);
+                            dir = TurnNorth;
+                        }
+                        else
+                        {
+                            right.speed(1);
+                            left.speed(1);
+                        }
+                    }
+                    else
+                    {
+                        //It's going north!
+                        right.speed(1);
+                        left.speed(1);
+                    }
                 }
             break;
             case Right:
-                if (avgLeft>=50 || avgFront<=40)
+                printf("TURN!\n\r");
+                if (/*avgLeft>=50 ||*/ avgFront<=40)
                 {
                     right.speed(-1);
                     left.speed(0);
@@ -110,6 +152,21 @@
                     dir=Forward;
                 }
             break;
+            case TurnNorth:
+                printf("TO THE NORTH!\n\r");
+                if (globalDir>330 || globalDir < 30)
+                {
+                    //It's going north!!
+                    right.speed(1);
+                    left.speed(1);
+                    dir=Forward;
+                }
+                else
+                {
+                    right.speed(.5);
+                    left.speed(-.5);
+                }
+            break;
         }
         wait(0.01);
     }