File content as of revision 8:ce5b1bf38077:

#ifndef SENSORS_H
#define SENSORS_H

//#include "Core.h"

#include "ExponentialAverage.h"
#include "L3GD20.h"
#include "LSM303DLHC.h"


float ax, ay, az;
float mx, my, mz;
float gx, gy, gz;

L3GD20 gyro(p9, p10);
LSM303DLHC compass(p9, p10);
PwmOut ledF(p23);
PwmOut ledR(p21);
PwmOut ledL(p22);
AnalogIn SenseR(p15);
AnalogIn SenseL(p16);
AnalogIn SenseF(p17);
bool wallfront = false;
bool wallright = false;
bool wallleft = false;
float cal_R =0, cal_L =0, cal_F =0;
Timer t;

float linearize(float sensor)
{
 return abs(log(1-sensor));  
}    


bool wallFront()
{
    return frontExpAvg.average() > .90;
}
bool wallLeft()
{
    return linearize(SenseL.read()) < 5.2;
}
bool wallRight()
{
    return linearize(SenseR.read()) < 5.2;
}
void check_walls()
{
  wallLeft();
  wallRight();
  wallFront();      
}

void initSensors()
{
    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();
}

#endif