Sensors.h

00001 #ifndef SENSORS_H
00002 #define SENSORS_H
00003 
00004 //#include "Core.h"
00005 
00006 #include "ExponentialAverage.h"
00007 #include "L3GD20.h"
00008 #include "LSM303DLHC.h"
00009 
00010 
00011 float ax, ay, az;
00012 float mx, my, mz;
00013 float gx, gy, gz;
00014 
00015 L3GD20 gyro(p9, p10);
00016 LSM303DLHC compass(p9, p10);
00017 PwmOut ledF(p23);
00018 PwmOut ledR(p21);
00019 PwmOut ledL(p22);
00020 AnalogIn SenseR(p15);
00021 AnalogIn SenseL(p16);
00022 AnalogIn SenseF(p17);
00023 bool wallfront = false;
00024 bool wallright = false;
00025 bool wallleft = false;
00026 float cal_R =0, cal_L =0, cal_F =0;
00027 Timer t;
00028 
00029 float linearize(float sensor)
00030 {
00031  return abs(log(1-sensor));  
00032 }    
00033 
00034 
00035 bool wallFront()
00036 {
00037     return frontExpAvg.average() > .90;
00038 }
00039 bool wallLeft()
00040 {
00041     return linearize(SenseL.read()) < 5.2;
00042 }
00043 bool wallRight()
00044 {
00045     return linearize(SenseR.read()) < 5.2;
00046 }
00047 void check_walls()
00048 {
00049   wallLeft();
00050   wallRight();
00051   wallFront();      
00052 }
00053 
00054 void initSensors()
00055 {
00056     for(int i = 0; i < 100; i++)
00057     {
00058         float l = SenseL.read();
00059         float r = SenseR.read();
00060         float f = SenseF.read();
00061         
00062         /*
00063         leftWeightedAvg.add(l);
00064         rightWeightedAvg.add(r);
00065         frontWeightedAvg.add(f);
00066         
00067 
00068         leftExpAvg.add(l);
00069         frontExpAvg.add(f);
00070         */
00071         leftExpAvg.add(l);
00072         rightExpAvg.add(r);
00073         frontExpAvg.add(f);
00074         //leftBufferAvg.add(l);
00075         //rightBufferAvg.add(r);
00076         //frontBufferAvg.add(f);
00077         
00078         //WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.average());
00079     }
00080         cal_R = abs(log(1-rightExpAvg.average()));
00081         cal_L = abs(log(1-leftExpAvg.average()));
00082         //cal_F = abs(log(1-frontExpAvg.average()));
00083         //float avgRight = rightExpAvg.average();
00084 }
00085 
00086 #endif