IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

main.cpp

Committer:
vsavkin3
Date:
2012-10-04
Revision:
4:9011798d1a20
Parent:
3:4ee5247f47ec
Child:
5:99545cf4dff4

File content as of revision 4:9011798d1a20:

#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);
    }
}