Diff: main.cpp

Revision:

1:6812f830e5a1

Parent:

0:1a0fa585e36c

Child:

2:25d2332ec855

Child:

3:4ee5247f47ec

--- a/main.cpp	Tue Oct 02 19:33:05 2012 +0000
+++ b/main.cpp	Thu Oct 04 19:11:02 2012 +0000
@@ -3,18 +3,103 @@
 
 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;
+    int readLeftVals[10];
+    int readRightVals[10];
+    int 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=10/readLeft;
+        
+        readRight=irRight;
+        readRight=10/readRight;
+        
+        readFront=irFront;
+        readFront=10/readFront;
+        
+        readFrontVals[i] = readFront;
+        readRightVals[i] = readRight;
+        readLeftVals[i] = readLeft;
+    }
+    
     while (1){
-        for (float s= -1.0; s < 1.0 ; s += 0.01) {
-           right.speed(s); 
-           left.speed(s);
-           wait(0.02);
-        }        
-        for (float s= 1.0; s > -1.0 ; s -= 0.01) {
-           right.speed(s); 
-           left.speed(s);
-           wait(0.02);
+        readLeft=irLeft;
+        readLeft=10/readLeft;
+        
+        readRight=irRight;
+        readRight=10/readRight;
+        
+        readFront=irFront;
+        readFront=10/readFront;
+        
+        
+       
+        
+        if (i>10)
+        {
+            i=0;
+        }
+        readFrontVals[i] = readFront;
+        readRightVals[i] = readRight;
+        readLeftVals[i] = readLeft;
+        i++;
+        
+        int 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);
     }
 }