File content as of revision 4:ecfe2115e9a4:

//#include "Core.h"

#include "Sensors.h"
#include "Motors.h"
#include "Communication.h"
#include "Flooding.h"

#include "BufferAverage.h"
#include "ExponentialAverage.h"
//#include "WeightedAverage.h"

int main()
{
    Motor.baud(115200);
    ledF.period_us(26.3); //28.5 //27.9
    wait_us(600);
    ledF.write(.5);
    stop_mov();
    
    
    wait(3);
    /*
    
    
    STEVE!!!!!!
    WHENEVER YOU ARE IN A CELL AND YOU WANT TO KNOW WHAT THE NEXT BEST MOVE IS
    USE THE flood_findPath(x, y)  where x and y are your current possition
    
    the function will return a MOVE type
    and you can use something similar to this:
    
    Move k = flood_findPath(xPos, yPos);
    switch(k)
    {
        case M_NORTH:
            moveNORTH();
            break;
     
        case M_SOUTH:
            moveSOUTH();
            break;
     
        case M_WEST:
            moveWEST();
            break;
        
        case M_EAST:
            moveEAST();
            break;             
        
    }   
    
    i've written the code for the following:
    
    
    moveNORTH()
    moveSOUTH()
    moveEAST()
    moveWEST()
    
    they are at the bottom of the page. take a look and read them.
    
    
    REMEMBER TO MOVE THEM TO THE TOP OF THE PAGE IF YOU PLAN TO USE THEM IN THE MAIN FUNCTION, OR VICEVERSA MOVE THE MAIN FUNC.
    TO THE BOTTOM
    
    */

    
    for(int i = 0; i < 100; i++)
    {
        float l = SenseL.read();
        float r = SenseR.read();
        float f = SenseF.read();
        
        /*
        leftWeightedAvg.add(l);
        rightWeightedAvg.add(r);
        frontWeightedAvg.add(f);
        

        leftExpAvg.add(l);
        frontExpAvg.add(f);
        */
        leftExpAvg.add(l);
        rightExpAvg.add(r);
        frontExpAvg.add(f);
        //leftBufferAvg.add(l);
        //rightBufferAvg.add(r);
        //frontBufferAvg.add(f);
        
        WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.average());
    }
        cal_R = abs(log(1-rightExpAvg.average()));
        cal_L = abs(log(1-leftExpAvg.average()));
        //cal_F = abs(log(1-frontExpAvg.average()));
        //float avgRight = rightExpAvg.average();
    
    
    setRightSpeed(3);
    setLeftSpeed(3);
    
    
    //while (true)
    while(SenseF.read() < 0.99)
    //while(frontExpAvg.average() < .90 ) //&& wallRight() && wallLeft()
    { 
        frontExpAvg.add(SenseF.read());
    
        //WIRELESS.printf("R: %f  L: %f\n\r", cal_R, cal_L);
        float PIDv = PID(abs(log(1-SenseR.read())), abs(log(1-SenseL.read())));
        //WIRELESS.printf("ERR: %f  Front: %f\n\r", PIDv, SenseF.read());
        
        if(PIDv < -0.1)
        {
            setRightSpeed(3);
            setLeftSpeed(4);
            wait_ms(20); 
              
        }
        else if(PIDv > .1)
        {
            setRightSpeed(4); //** Just flipped these values
            setLeftSpeed(3);
            wait_ms(20);        
        }
        else
        {
            setRightSpeed(3);
            setLeftSpeed(3);
        } 
        //check_walls();    
    }
    
    //setRightSpeed(-2);
    //setLeftSpeed(-2);
    
    //stop_mov();
    wait_ms(20);
    handbrake();   
    return 0;
}
void moveNORTH()
{
    switch(orientation)
    {
        case NORTH:
            // go forward...
            break;
        case WEST:
            // turn right then forward
            break;            
        case SOUTH:
            // turn around then forward
            break;            
        case EAST:
            // turn left then forward
            break;            
    }
}
void moveSOUTH()
{
    switch(orientation)
    {
        case NORTH:
            // turn around then forward...
            break;
        case WEST:
            // turn left then forward
            break;            
        case SOUTH:
            // go forward
            break;            
        case EAST:
            // turn right then forward
            break;            
    }
}
void moveEAST()
{
    switch(orientation)
    {
        case NORTH:
            // turn right then forward...
            break;
        case WEST:
            // turn around then foward
            break;            
        case SOUTH:
            // turn left then forward
            break;            
        case EAST:
            // go forward
            break;            
    }
}
void moveWEST()
{
    switch(orientation)
    {
        case NORTH:
            // turn left then forward...
            break;
        case WEST:
            // go forward
            break;            
        case SOUTH:
            // turn left then forward
            break;            
        case EAST:
            // turn around then forward
            break;            
    }
}