File content as of revision 0:79ae0b074fbe:

/* 
* DISTHDL, Distance sensor handler 
* Author: Jose Alberto Pesado Santiago
* ITESM, MSE, Instrumentacion
* Rev 1.0, 2014, Oct 13th
*/

#include "mbed.h"

#define FILTER_LENGTH	10	/* number of samples taken */

/* Caracterization table */
int InTblADCLeft[] = { 6000,6600,6850,7260,7800,8140,8500,9500,10000,11200,12300,13300,15000,16600,19000,22300,25500,31300,41300,54500,65500 };
int OutTblCmLeft[] = { 44,40,38,36,34,32,30,28,26,24,22,20,18,16,14,12,10,8,6,4,2 };

int InTblADCRight[] = { 5500,5900,6300,6800,7200,7600,8000,8400,9000,10000,11000,12300,13300,15500,18100,21100,24000,29500,38000,51500,64500 };
int OutTblCmRight[] = { 44,40,38,36,34,32,30,28,26,24,22,20,18,16,14,12,10,8,6,4,2 };

#if 0
typedef struct
{
	int InTbl[21];
	int OutTbl[21];
	AnalogIn Sensor;
	int Samples[FILTER_LENGTH];
	int FilteredInput;
	int Distance;
}
tsDistanceSensor;

tsDistanceSensor SensorLeft = {
	{6000,6600,6850,7260,7800,8140,8500,9500,10000,11200,12300,13300,15000,16600,19000,22300,25500,31300,41300,54500,65500},
	{44,40,38,36,34,32,30,28,26,24,22,20,18,16,14,12,10,8,6,4,2},
	SensorLeft(A0),
	{0,0,0,0,0,0,0,0,0,0},
	0,
	0,
}
#endif
/* Sensor input */
AnalogIn SensorLeft(A0);
AnalogIn SensorRight(A1);

/* Serial communication to the PC */
Serial pc(USBTX, USBRX);

int multiMap(int val, int* _in, int* _out, uint8_t size);
int interpolate(int x, int xLeft, int xRight, int yBottom, int yUp);

int main()
{
    long int InputSensorLeft = 0;
	long int DistanceLeft = 0;
    int InputSensorSamplesLeft[FILTER_LENGTH];
    
    long int InputSensorRight = 0;
	long int DistanceRight = 0;
    int InputSensorSamplesRight[FILTER_LENGTH];
    
    int i = 0;
    int j = 0;

    /* Initiates communication with PC */
    pc.printf("Temp sensor:\n");

    while (true) {
        wait(0.1f); // Reads will be made every 50ms

        /* Filtered input */
		if(i<FILTER_LENGTH)
		{
			/* The filter buffer is not full */
			InputSensorSamplesLeft[i] = SensorLeft.read_u16();
			InputSensorSamplesRight[i] = SensorRight.read_u16();
			i++;
		}
		else
		{
			/* The filter buffer is complete, newest sample is set in the first position, the past samples are shifted */
			for(j=0;j<FILTER_LENGTH-1;j++)
			{
				InputSensorSamplesLeft[j] = InputSensorSamplesLeft[j+1];
				InputSensorSamplesRight[j] = InputSensorSamplesRight[j+1];
			}
			InputSensorSamplesLeft[j] = SensorLeft.read_u16();
			InputSensorSamplesRight[j] = SensorRight.read_u16();
		}   
		/* Average of all the available samples */
		for(j=0;j<i;j++)
		{
			InputSensorLeft += InputSensorSamplesLeft[j];
			InputSensorRight += InputSensorSamplesRight[j];
		}
		InputSensorLeft /= i;
		InputSensorRight /= i;
		
		DistanceLeft = multiMap((int)InputSensorLeft, InTblADCLeft, OutTblCmLeft, sizeof(InTblADCLeft)/sizeof(InTblADCLeft[0]));
		DistanceRight = multiMap((int)InputSensorRight, InTblADCLeft, OutTblCmRight, sizeof(InTblADCRight)/sizeof(InTblADCRight[0]));
		
		/* The current sensor inputs and telltale status is transmitted to the PC */        
        pc.printf("Sensor Left = %d\tDistance Left = %d\tSensor Right = %d\tDistance Right = %d\r\n", 
                    InputSensorLeft, DistanceLeft,InputSensorRight, DistanceRight); // print the value of input sensor
    }
}

int multiMap(int val, int* _in, int* _out, uint8_t size)
{
  // take care the value is within range
  // val = constrain(val, _in[0], _in[size-1]);
  if (val <= _in[0]) return _out[0];
  if (val >= _in[size-1]) return _out[size-1];

  // search right interval
  uint8_t pos = 1;  // _in[0] allready tested
  while(val > _in[pos]) pos++;

  // this will handle all exact "points" in the _in array
  if (val == _in[pos]) return _out[pos];

  // interpolate in the right segment for the rest
  return interpolate(val, _in[pos-1], _in[pos], _out[pos-1], _out[pos]);
}

int interpolate(int x, int xLeft, int xRight, int yBottom, int yUp)
{
  int y = 0;
  
  y = (x - xLeft) * ((yUp - yBottom)/(xRight - xLeft)) + yBottom;
  
  return(y);
}