Caracterization of Sharp GP2D120 distance sensors (left and right). These sensors show high sensitivity to different ambient light conditions and position regaring the reflecting object. An averaging filter was implemented to reduce such variations, however more investigation is required to improve the reliability of the measured distances. Currently the error in the measured distance is about +/- 4 cm.
Dependencies: mbed
main.cpp
00001 /* 00002 * DISTHDL, Distance sensor handler 00003 * Author: Jose Alberto Pesado Santiago 00004 * ITESM, MSE, Instrumentacion 00005 * Rev 1.0, 2014, Oct 13th 00006 */ 00007 00008 #include "mbed.h" 00009 00010 #define FILTER_LENGTH 10 /* number of samples taken */ 00011 00012 /* Caracterization table */ 00013 int InTblADCLeft[] = { 6000,6600,6850,7260,7800,8140,8500,9500,10000,11200,12300,13300,15000,16600,19000,22300,25500,31300,41300,54500,65500 }; 00014 int OutTblCmLeft[] = { 44,40,38,36,34,32,30,28,26,24,22,20,18,16,14,12,10,8,6,4,2 }; 00015 00016 int InTblADCRight[] = { 5500,5900,6300,6800,7200,7600,8000,8400,9000,10000,11000,12300,13300,15500,18100,21100,24000,29500,38000,51500,64500 }; 00017 int OutTblCmRight[] = { 44,40,38,36,34,32,30,28,26,24,22,20,18,16,14,12,10,8,6,4,2 }; 00018 00019 #if 0 00020 typedef struct 00021 { 00022 int InTbl[21]; 00023 int OutTbl[21]; 00024 AnalogIn Sensor; 00025 int Samples[FILTER_LENGTH]; 00026 int FilteredInput; 00027 int Distance; 00028 } 00029 tsDistanceSensor; 00030 00031 tsDistanceSensor SensorLeft = { 00032 {6000,6600,6850,7260,7800,8140,8500,9500,10000,11200,12300,13300,15000,16600,19000,22300,25500,31300,41300,54500,65500}, 00033 {44,40,38,36,34,32,30,28,26,24,22,20,18,16,14,12,10,8,6,4,2}, 00034 SensorLeft(A0), 00035 {0,0,0,0,0,0,0,0,0,0}, 00036 0, 00037 0, 00038 } 00039 #endif 00040 /* Sensor input */ 00041 AnalogIn SensorLeft(A0); 00042 AnalogIn SensorRight(A1); 00043 00044 /* Serial communication to the PC */ 00045 Serial pc(USBTX, USBRX); 00046 00047 int multiMap(int val, int* _in, int* _out, uint8_t size); 00048 int interpolate(int x, int xLeft, int xRight, int yBottom, int yUp); 00049 00050 int main() 00051 { 00052 long int InputSensorLeft = 0; 00053 long int DistanceLeft = 0; 00054 int InputSensorSamplesLeft[FILTER_LENGTH]; 00055 00056 long int InputSensorRight = 0; 00057 long int DistanceRight = 0; 00058 int InputSensorSamplesRight[FILTER_LENGTH]; 00059 00060 int i = 0; 00061 int j = 0; 00062 00063 /* Initiates communication with PC */ 00064 pc.printf("Temp sensor:\n"); 00065 00066 while (true) { 00067 wait(0.1f); // Reads will be made every 50ms 00068 00069 /* Filtered input */ 00070 if(i<FILTER_LENGTH) 00071 { 00072 /* The filter buffer is not full */ 00073 InputSensorSamplesLeft[i] = SensorLeft.read_u16(); 00074 InputSensorSamplesRight[i] = SensorRight.read_u16(); 00075 i++; 00076 } 00077 else 00078 { 00079 /* The filter buffer is complete, newest sample is set in the first position, the past samples are shifted */ 00080 for(j=0;j<FILTER_LENGTH-1;j++) 00081 { 00082 InputSensorSamplesLeft[j] = InputSensorSamplesLeft[j+1]; 00083 InputSensorSamplesRight[j] = InputSensorSamplesRight[j+1]; 00084 } 00085 InputSensorSamplesLeft[j] = SensorLeft.read_u16(); 00086 InputSensorSamplesRight[j] = SensorRight.read_u16(); 00087 } 00088 /* Average of all the available samples */ 00089 for(j=0;j<i;j++) 00090 { 00091 InputSensorLeft += InputSensorSamplesLeft[j]; 00092 InputSensorRight += InputSensorSamplesRight[j]; 00093 } 00094 InputSensorLeft /= i; 00095 InputSensorRight /= i; 00096 00097 DistanceLeft = multiMap((int)InputSensorLeft, InTblADCLeft, OutTblCmLeft, sizeof(InTblADCLeft)/sizeof(InTblADCLeft[0])); 00098 DistanceRight = multiMap((int)InputSensorRight, InTblADCLeft, OutTblCmRight, sizeof(InTblADCRight)/sizeof(InTblADCRight[0])); 00099 00100 /* The current sensor inputs and telltale status is transmitted to the PC */ 00101 pc.printf("Sensor Left = %d\tDistance Left = %d\tSensor Right = %d\tDistance Right = %d\r\n", 00102 InputSensorLeft, DistanceLeft,InputSensorRight, DistanceRight); // print the value of input sensor 00103 } 00104 } 00105 00106 int multiMap(int val, int* _in, int* _out, uint8_t size) 00107 { 00108 // take care the value is within range 00109 // val = constrain(val, _in[0], _in[size-1]); 00110 if (val <= _in[0]) return _out[0]; 00111 if (val >= _in[size-1]) return _out[size-1]; 00112 00113 // search right interval 00114 uint8_t pos = 1; // _in[0] allready tested 00115 while(val > _in[pos]) pos++; 00116 00117 // this will handle all exact "points" in the _in array 00118 if (val == _in[pos]) return _out[pos]; 00119 00120 // interpolate in the right segment for the rest 00121 return interpolate(val, _in[pos-1], _in[pos], _out[pos-1], _out[pos]); 00122 } 00123 00124 int interpolate(int x, int xLeft, int xRight, int yBottom, int yUp) 00125 { 00126 int y = 0; 00127 00128 y = (x - xLeft) * ((yUp - yBottom)/(xRight - xLeft)) + yBottom; 00129 00130 return(y); 00131 } 00132
Generated on Thu Jul 21 2022 21:54:47 by
1.7.2