00001
00002 #include "unit_conversion.h"
00003
00004
00005
00006
00007
00008
00009 const float CM_PER_US = 0.0343;
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 const float AD_MULT = 3.3 / 65472.0;
00026 const float A = -26.13019157;
00027 const float B = 153.7504349;
00028 const float C = -312.5940834;
00029 const float D = 250.6868629;
00030
00031
00032
00033
00034 static float DIVIDER_RATIO = 2.17 / (2.17 + 11.87);
00035 float getBatteryVoltage(int ad_reading) {
00036 return ad_reading * AD_MULT / DIVIDER_RATIO;
00037 }
00038
00039 static float x = 0;
00040 float getInfraredRangefinderCM(int ad_reading) {
00041 x = (float)ad_reading * AD_MULT;
00042 return A * pow(x, 3) + B * pow(x, 2) + C * x + D;
00043 }
00044
00045
00046 float getSonarRangefinderCM(int pulsin_reading) {
00047 return pulsin_reading / 2 * CM_PER_US;
00048 }
00049