File content as of revision 6:2b89c05628ce:

#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);
HMC6352 compass(p9, p10);

enum direction{ Right, Forward, Left, TurnNorth };

int main() {
    enum direction dir=Forward;
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    
    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", 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");
                    //wait(0.1);
                    dir=Right;
                }
                else
                {
                    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:
                printf("TURN!\n\r");
                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;
            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);
    }
}