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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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