Basic library for Pololu's Sharp GP2Y0A51SK0F
Dependencies: mbed
Dependents: GrabTest R5 2016 Robotics Team 1
Diff: DistanceSensor.cpp
- Revision:
- 1:8db052ec94f1
- Parent:
- 0:d03df6dd14d6
diff -r d03df6dd14d6 -r 8db052ec94f1 DistanceSensor.cpp --- a/DistanceSensor.cpp Wed Apr 06 23:02:33 2016 +0000 +++ b/DistanceSensor.cpp Thu Apr 07 23:06:50 2016 +0000 @@ -1,12 +1,35 @@ #include "DistanceSensor.h" #include <cmath> -DistanceSensor::DistanceSensor(PinName ai) : voltage(ai) {} +DistanceSensor::DistanceSensor(PinName ai, int profNum) : voltage(ai) +{ + if(profNum > -1 && profNum < 4) + prof = profNum; + else + prof = 1; +} -float DistanceSensor::getDistance() +double DistanceSensor::getDistance() { - float v = voltage.read(); - //calibration curve for 2 - return 1044.8631*pow(v,5) - 1497.4521*pow(v,4) + 473.2921*pow(v,3) - + 199.07*pow(v,2) - 147.1165*v + 28.1844; + double v = voltage.read(); + double dist; + double k1 = 1.1577767608; + // double k2; + double k3 = 1.1672866347; + double p1 = -1.1971310749; + // double p2; + double p3 = -1.2007193388; + + switch(prof) + { + case(1) : dist = k1*pow(v, p1); + break; + case(2) : dist = 0; // placeholder until unit 2 gets redone + break; + case(3) : dist = k3*pow(v, p3); + break; + default : dist = 0; + } + + return dist; } \ No newline at end of file