File content as of revision 8:c7d277d05f23:

#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);
AnalogIn dirLeft(p17);
AnalogIn dirRight(p16);
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 j=0;
    float readLeft;
    float readRight;
    float readFront;
    float diagRight;
    float diagLeft;
    float stable[5][4];
        readLeft=irLeft;
        readRight=irRight;
        readFront=irFront;
        diagRight=dirRight;
        diagLeft=dirLeft;
    
    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;
        j=1;
    while (1){
        if(j>=4) j=0;
       
        readLeft=irLeft;
        readRight=irRight;
        readFront=irFront;
        diagRight=dirRight;
        diagLeft=dirLeft;
        readLeft=21/readLeft;
        readRight=21/readRight;
        readFront=21/readFront;
        diagRight=21/diagRight;
        diagLeft=21/diagLeft;
        float avgFront = 0, avgRight = 0, avgLeft = 0;
        avgFront = readFront;
        avgRight = readRight;
        avgLeft  = readLeft;
stable[0][j] = avgLeft;
stable[1][j] = avgRight;
stable[2][j] = diagLeft;
stable[3][j] = diagRight;
stable[4][j] = avgFront;

j++;
        printf("Front: %5f Right: %5f Left: %5f dLeft: %5f dRight: %5f", avgFront, avgRight, avgLeft,diagLeft,diagRight);
        float globalDir = compass.sample()/10.0;
        printf(" Heading: %f\n\r", globalDir);

        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 ((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)*/)
                {
                    right.speed(0);
                    left.speed(0);
                    printf("stopped!\n\r");
                    //wait(0.1);
                    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))*/)
                {
                    right.speed(0);
                    left.speed(0);
                    printf("stopped2!\n\r");
                    //wait(0.1);
                    NorthOn=0;
                    dir=Left;
                }
                else if(
                (((globalDir > 30 && globalDir <= 180)||(globalDir > 180 && globalDir <= 330)&&NorthOn)
         )
                )
                {
                            right.speed(0);
                            left.speed(0);
                            dir = TurnNorth;
                }
                else {right.speed(0.95); left.speed(1);
                printf("CHARGE!\n\r");
                }
            break;
            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))
                )
                {
                    right.speed(-1);
                    left.speed(0);
                   // 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");
                    //wait(0.1);
                    dir=Forward;
                }
            break;
            
            case Left:
                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)
                   )
                {
                    right.speed(0);
                    left.speed(-1);
                   // 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");
                    //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(0);
                    left.speed(0);
                    dir=Forward;
                }
                else
                {   if(globalDir<=180){
                    right.speed(1);
                    left.speed(-1);}
                    else{right.speed(-1); left.speed(1);}
                }
            break;
        }
        wait(0.01);
    }
}