Diff: main.cpp

Revision:

4:9011798d1a20

Parent:

3:4ee5247f47ec

Child:

5:99545cf4dff4

--- a/main.cpp	Thu Oct 04 20:45:34 2012 +0000
+++ b/main.cpp	Thu Oct 04 22:05:11 2012 +0000
@@ -1,105 +1,111 @@
-#include "mbed.h"
-#include "Motor.h"
-
-Motor right(p21, p23, p22); // pwm, fwd, rev
-Motor left(p26, p24, p25); // pwm, fwd, rev
-AnalogIn irLeft(p20);
-AnalogIn irFront(p19);
-AnalogIn irRight(p18);
-AnalogIn sonarLeft(p17);
-
-enum direction{ Right, Forward };
-
-int main() {
-    enum direction dir=Forward;
-    
-    printf("Front Right Left\n\r");
-    int i=0;
-    float readLeftVals[10];
-    float readRightVals[10];
-    float readFrontVals[10];
-    float readLeft;
-    float readRight;
-    float readFront;
-    
-    //Initialize the averager before the robot even moves
-    for (int j=0; j<10; j++);
-    {
-        readLeft=irLeft;
-        readLeft=21/readLeft;
-        
-        readRight=irRight;
-        readRight=21/readRight;
-        
-        readFront=irFront;
-        readFront=21/readFront;
-        
-        readFrontVals[i] = readFront;
-        readRightVals[i] = readRight;
-        readLeftVals[i] = readLeft;
-    }
-    
-    while (1){
-        readLeft=irLeft;
-        readLeft=21/readLeft;
-        
-        readRight=irRight;
-        readRight=21/readRight;
-        
-        readFront=irFront;
-        readFront=21/readFront;
-        
-        
-       
-        
-        if (i>=10)
-        {
-            i=0;
-        }
-        readFrontVals[i] = readFront;
-        readRightVals[i] = readRight;
-        readLeftVals[i] = readLeft;
-        i++;
-        
-        float avgFront = 0, avgRight = 0, avgLeft = 0;
-        
-        for (int j=0; j<10; j++)
-        {
-            avgFront += readFrontVals[j]/10;
-            avgLeft += readLeftVals[j]/10;
-            avgRight += readRightVals[j]/10;
-        }
-        
-        printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft);
-        
-        switch(dir){
-            case Forward:
-                if (avgFront <= 18)
-                {
-                    wait(0.5);
-                    dir=Right;
-                    printf("less than 18\n\r");
-                }
-                else
-                {
-                    right.speed(1);
-                    left.speed(1);
-                }
-            break;
-            case Right:
-                if (avgLeft>30 || avgFront<=18)
-                {
-                    right.speed(-1);
-                    left.speed(1);
-                    printf("turning!\n\r");
-                }
-                else 
-                {
-                    wait(0.5);
-                    dir=Forward;
-                }
-            break;
-        }
-        wait(0.01);
-    }
-}
+#include "mbed.h"
+#include "Motor.h"
+#include "HMC6352.h"
+
+Motor right(p21, p23, p22); // pwm, fwd, rev
+Motor left(p26, p24, p25); // pwm, fwd, rev
+AnalogIn irLeft(p20);
+AnalogIn irFront(p19);
+AnalogIn irRight(p18);
+
+enum direction{ Right, Forward };
+
+int main() {
+    enum direction dir=Forward;
+    
+    printf("Front Right Left\n\r");
+    int i=0;
+    float readLeftVals[10];
+    float readRightVals[10];
+    float readFrontVals[10];
+    float readLeft;
+    float readRight;
+    float readFront;
+    
+    //Initialize the averager before the robot even moves
+    for (int j=0; j<10; j++);
+    {
+        readLeft=irLeft;
+        readLeft=21/readLeft;
+        
+        readRight=irRight;
+        readRight=21/readRight;
+        
+        readFront=irFront;
+        readFront=21/readFront;
+        
+        readFrontVals[i] = readFront;
+        readRightVals[i] = readRight;
+        readLeftVals[i] = readLeft;
+    }
+    
+    while (1){
+        readLeft=irLeft;
+        readLeft=21/readLeft;
+        
+        readRight=irRight;
+        readRight=21/readRight;
+        
+        readFront=irFront;
+        readFront=21/readFront;
+        
+       
+        
+        if (i>=10)
+        {
+            i=0;
+        }
+        readFrontVals[i] = readFront;
+        readRightVals[i] = readRight;
+        readLeftVals[i] = readLeft;
+        i++;
+        
+        float avgFront = 0, avgRight = 0, avgLeft = 0;
+        
+        for (int j=0; j<10; j++)
+        {
+            avgFront += readFrontVals[j]/10;
+            avgLeft += readLeftVals[j]/10;
+            avgRight += readRightVals[j]/10;
+        }
+        avgFront = readFront;
+        avgRight = readRight;
+        //printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft);
+        
+        switch(dir){
+            case Forward:
+                if (avgFront <= 40)
+                {
+                    right.speed(0);
+                    left.speed(0);
+                   // printf("stopped!\n\r");
+                    //wait(0.1);
+                    dir=Right;
+                   // printf("less than 18\n\r");
+                }
+                else
+                {
+                    right.speed(1);
+                    left.speed(1);
+                }
+            break;
+            case Right:
+                if (avgLeft>=50 || avgFront<=40)
+                {
+                    right.speed(-1);
+                    left.speed(0);
+                   // printf("turning!\n\r");
+                }
+                else 
+                {
+                    right.speed(0);
+                    left.speed(0);
+                  //  printf("stopped!\n\r");
+                    //wait(0.1);
+                    dir=Forward;
+                }
+            break;
+        }
+        wait(0.01);
+    }
+}