ReadSensor.cpp

00001 #include "mbed.h"
00002 
00003 AnalogIn Sensor_L(PA_0);
00004 AnalogIn Sensor_F(PA_1);
00005 AnalogIn Sensor_R(PA_4);
00006 
00007 float readSensorValue(int);
00008 
00009 bool readSensor(int Position)
00010 {
00011     float dist1, dist2;
00012     while(true) {
00013         dist1 = readSensorValue(Position);
00014         //wait(0.3);
00015         dist2 = readSensorValue(Position);
00016         if(abs(dist1-dist2) < 3.0f) {
00017             if(((dist1+dist2)/2) >= 13.0f) {
00018                 return false;
00019             } else {
00020                 return true;
00021             }
00022         }
00023     }
00024 }
00025 
00026 float readSensorValue(int Position)
00027 {
00028 
00029     float distance;
00030     float voltage;
00031     float distancetot = 0;
00032     int i = 0;
00033 
00034     while(true) {
00035         for(i = 0; i < 2; i++) {
00036             switch(Position) {
00037                 case 1:
00038                     voltage = Sensor_L.read();
00039                     //distance = -4122.2*pow(voltage,5)+9496.2*pow(voltage,4)-8519.4*pow(voltage,3)+3743.5*pow(voltage,2)-822.5*voltage+77.9;
00040                     distance = 306.4*pow(voltage,5)-405.41*pow(voltage,4)+36.17*pow(voltage,3)+177.5*pow(voltage,2)-106.65*voltage+23.08;
00041                     if(distance > 15.0)
00042                     {
00043                         distance = 15.0;
00044                     }
00045                     if(distance < 2)
00046                     {
00047                         distance = 2.0;
00048                     } 
00049                     break;
00050 
00051                 case 2:
00052                     voltage = Sensor_F.read();
00053                     
00054                     //distance = -4122*pow(voltage,5)+9496*pow(voltage,4)-8519*pow(voltage,3)+3743*pow(voltage,2)-822*voltage;
00055                     distance = -137.67*pow(voltage,5)+129.58*pow(voltage,4)-55.36*pow(voltage,3)+72.25*pow(voltage,2)-59.54*voltage+18.0;
00056                                         if(distance > 15.0)
00057                     {
00058                         distance = 15.0;
00059                     }
00060                     if(distance < 2)
00061                     {
00062                         distance = 2.0;
00063                     } 
00064                     break;
00065 
00066                 case 3:
00067                     voltage = Sensor_R.read();
00068                     //printf("Spannung, %f\n",voltage);
00069                     distance = 3407.9*pow(voltage,5)-5715*pow(voltage,4)+3435.4*pow(voltage,3)-828.2*pow(voltage,2)+29.5*voltage+16.1;
00070                     if(distance > 15.0)
00071                     {
00072                         distance = 15.0;
00073                     }
00074                     if(distance < 2)
00075                     {
00076                         distance = 2.0;
00077                     } 
00078                     break;
00079 
00080                 default:
00081                     return true;
00082                     break;
00083             }
00084             distancetot = distancetot + distance;
00085         }
00086         //ticker_sens.detach();
00087         distancetot = distancetot / (i);
00088         //printf("%f\n",distancetot);
00089         return distancetot;
00090         //filter.in(distance);
00091         //filteredDistance = filter.out();
00092 
00093 
00094     }
00095 }