Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Headers/Sensors.h@8:ce5b1bf38077, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 23:58:04 2014 +0000
Revision:
8:ce5b1bf38077
Parent:
6:6e96e93689df 
asdf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
goy5022 0:c2ec30f28676 1 #ifndef SENSORS_H
goy5022 0:c2ec30f28676 2 #define SENSORS_H
goy5022 0:c2ec30f28676 3
goy5022 2:997f57aee3b7 4 //#include "Core.h"
goy5022 2:997f57aee3b7 5
goy5022 2:997f57aee3b7 6 #include "ExponentialAverage.h"
goy5022 0:c2ec30f28676 7 #include "L3GD20.h"
goy5022 0:c2ec30f28676 8 #include "LSM303DLHC.h"
goy5022 0:c2ec30f28676 9
goy5022 2:997f57aee3b7 10
goy5022 2:997f57aee3b7 11 float ax, ay, az;
goy5022 2:997f57aee3b7 12 float mx, my, mz;
goy5022 2:997f57aee3b7 13 float gx, gy, gz;
goy5022 2:997f57aee3b7 14
goy5022 0:c2ec30f28676 15 L3GD20 gyro(p9, p10);
goy5022 0:c2ec30f28676 16 LSM303DLHC compass(p9, p10);
goy5022 1:cfe6a6ad8dca 17 PwmOut ledF(p23);
goy5022 1:cfe6a6ad8dca 18 PwmOut ledR(p21);
goy5022 1:cfe6a6ad8dca 19 PwmOut ledL(p22);
goy5022 1:cfe6a6ad8dca 20 AnalogIn SenseR(p15);
goy5022 1:cfe6a6ad8dca 21 AnalogIn SenseL(p16);
goy5022 1:cfe6a6ad8dca 22 AnalogIn SenseF(p17);
goy5022 3:1a8a7cc709cc 23 bool wallfront = false;
goy5022 3:1a8a7cc709cc 24 bool wallright = false;
goy5022 3:1a8a7cc709cc 25 bool wallleft = false;
goy5022 4:ecfe2115e9a4 26 float cal_R =0, cal_L =0, cal_F =0;
goy5022 6:6e96e93689df 27 Timer t;
goy5022 2:997f57aee3b7 28
goy5022 5:9e504a5a1f48 29 float linearize(float sensor)
goy5022 5:9e504a5a1f48 30 {
goy5022 5:9e504a5a1f48 31 return abs(log(1-sensor));
goy5022 5:9e504a5a1f48 32 }
goy5022 5:9e504a5a1f48 33
goy5022 2:997f57aee3b7 34
goy5022 2:997f57aee3b7 35 bool wallFront()
goy5022 2:997f57aee3b7 36 {
goy5022 2:997f57aee3b7 37 return frontExpAvg.average() > .90;
goy5022 2:997f57aee3b7 38 }
goy5022 2:997f57aee3b7 39 bool wallLeft()
goy5022 1:cfe6a6ad8dca 40 {
goy5022 5:9e504a5a1f48 41 return linearize(SenseL.read()) < 5.2;
goy5022 1:cfe6a6ad8dca 42 }
goy5022 2:997f57aee3b7 43 bool wallRight()
goy5022 2:997f57aee3b7 44 {
goy5022 5:9e504a5a1f48 45 return linearize(SenseR.read()) < 5.2;
goy5022 2:997f57aee3b7 46 }
goy5022 4:ecfe2115e9a4 47 void check_walls()
goy5022 4:ecfe2115e9a4 48 {
goy5022 4:ecfe2115e9a4 49 wallLeft();
goy5022 4:ecfe2115e9a4 50 wallRight();
goy5022 6:6e96e93689df 51 wallFront();
goy5022 4:ecfe2115e9a4 52 }
goy5022 6:6e96e93689df 53
goy5022 6:6e96e93689df 54 void initSensors()
goy5022 6:6e96e93689df 55 {
goy5022 6:6e96e93689df 56 for(int i = 0; i < 100; i++)
goy5022 6:6e96e93689df 57 {
goy5022 6:6e96e93689df 58 float l = SenseL.read();
goy5022 6:6e96e93689df 59 float r = SenseR.read();
goy5022 6:6e96e93689df 60 float f = SenseF.read();
goy5022 6:6e96e93689df 61
goy5022 6:6e96e93689df 62 /*
goy5022 6:6e96e93689df 63 leftWeightedAvg.add(l);
goy5022 6:6e96e93689df 64 rightWeightedAvg.add(r);
goy5022 6:6e96e93689df 65 frontWeightedAvg.add(f);
goy5022 6:6e96e93689df 66
goy5022 6:6e96e93689df 67
goy5022 6:6e96e93689df 68 leftExpAvg.add(l);
goy5022 6:6e96e93689df 69 frontExpAvg.add(f);
goy5022 6:6e96e93689df 70 */
goy5022 6:6e96e93689df 71 leftExpAvg.add(l);
goy5022 6:6e96e93689df 72 rightExpAvg.add(r);
goy5022 6:6e96e93689df 73 frontExpAvg.add(f);
goy5022 6:6e96e93689df 74 //leftBufferAvg.add(l);
goy5022 6:6e96e93689df 75 //rightBufferAvg.add(r);
goy5022 6:6e96e93689df 76 //frontBufferAvg.add(f);
goy5022 6:6e96e93689df 77
goy5022 6:6e96e93689df 78 //WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.average());
goy5022 6:6e96e93689df 79 }
goy5022 6:6e96e93689df 80 cal_R = abs(log(1-rightExpAvg.average()));
goy5022 6:6e96e93689df 81 cal_L = abs(log(1-leftExpAvg.average()));
goy5022 6:6e96e93689df 82 //cal_F = abs(log(1-frontExpAvg.average()));
goy5022 6:6e96e93689df 83 //float avgRight = rightExpAvg.average();
goy5022 6:6e96e93689df 84 }
goy5022 6:6e96e93689df 85
goy5022 0:c2ec30f28676 86 #endif