Revision 0:79ae0b074fbe, committed 2014-10-14

Comitter:

josepesado

Date:

Tue Oct 14 04:05:48 2014 +0000

Commit message:

Caracterization of distance sensors (left and right). The GP2D120 sensors show high sensitivity to light conditions and angle respect the reflecting object. A n averaging filter was implemented to reduce unwanted variations. Further investigation is re...

Changed in this revision

diff -r 000000000000 -r 79ae0b074fbe main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 14 04:05:48 2014 +0000
@@ -0,0 +1,132 @@
+/* 
+* 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);
+}
+

diff -r 000000000000 -r 79ae0b074fbe mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Oct 14 04:05:48 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/552587b429a1
\ No newline at end of file