IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

main.cpp

Committer:
vsavkin3
Date:
2012-10-04
Revision:
2:25d2332ec855
Parent:
1:6812f830e5a1

File content as of revision 2:25d2332ec855:

#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++);
    {
        readFront=irFront;
        readLeft=irLeft;
        readRight=irRight;
        readLeft=readLeft*3.3/.0032;
        
        
        readRight=readRight*3.3/.0032;
        
        
        readFront=readFront*3.3/.0032;
        
        readFrontVals[i] = readFront;
        readRightVals[i] = readRight;
        readLeftVals[i] = readLeft;
    }
    
    while (1){
        readLeft=irLeft;
         readRight=irRight;
         readFront=irFront;
       readLeft=readLeft*3.3/.0032;
        
        
        readRight=readRight*3.3/.0032;
        
        
        readFront=readFront*3.3/.0032;
        
        
       
        
        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);
        
        avgFront = readFront;
        avgLeft = readLeft;
        switch(dir){
            case Forward:
                if (avgFront >= 520)
                {
                    left.speed(0);
                    right.speed(0);
                    dir=Right;
                    wait(0.5);
                    printf("less than 18\n\r");
                }
                else
                {
                    right.speed(1);
                    left.speed(1);
                }
            break;
            case Right:
                if (avgFront >= 470 || avgLeft<=510)
                { //if(avgFront <=480){dir=Forward; wait(0.5);}
                    right.speed(-1);
                    left.speed(1);
                    printf("turning!\n\r");
                
                }
                else
                {
                    
                    left.speed(0);
                    right.speed(0);
                    wait(0.5);
                    dir=Forward;
                }
            break;
        }
        wait(0.01);
    }
}