IRSensor.cpp

00001 
00002 //Implementation IR Sensoren
00003 // V04.18
00004 // V. Ahlers
00005 
00006 #include <cmath>
00007 #include "IRSensor.h"
00008 
00009 using namespace std;
00010 
00011     const float IRSensor :: PR1 = 3.4734*0.000000001;    //Koeffizienten
00012     const float IRSensor :: PR2 = -7.1846*0.000001;
00013     const float IRSensor :: PR3 = 0.0055;
00014     const float IRSensor :: PR4 = -1.9304;
00015     const float IRSensor :: PR5 = 301.2428;
00016     const float IRSensor :: PL1 = 3.4734*0.000000001;
00017     const float IRSensor :: PL2 = -7.1846*0.000001;
00018     const float IRSensor :: PL3 = 0.0055;
00019     const float IRSensor :: PL4 = -1.9304;
00020     const float IRSensor :: PL5 = 301.2428;
00021     const float IRSensor :: PF1 = 6.1767f*pow(10.0f,-10);
00022     const float IRSensor :: PF2 = -1.9975f*pow(10.0f,-6);
00023     const float IRSensor :: PF3 = 0.0024f;
00024     const float IRSensor :: PF4 = -1.3299f;
00025     const float IRSensor :: PF5 = 351.1557f;
00026     const int IRSensor :: minIrR = 0;   //Noch definieren
00027     const int IRSensor :: minIrL = 0;
00028     const int IRSensor :: minIrF = 0;
00029 
00030 
00031 
00032 // IR Sensor Distanz in mm
00033 IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F) : 
00034 IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F){}
00035                      
00036 IRSensor::~IRSensor(){}
00037 
00038 float IRSensor::readR() {
00039     
00040         measR = IrRight.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
00041         measR = measR * 1000; // Change the value to be in the 0 to 1000 range
00042         disR = PR1*pow(measR,4)+PR2*pow(measR,3)+PR3*pow(measR,2)+PR4*measR+PR5; //disR = f(measR)
00043         
00044         return disR;    
00045 }
00046 
00047 float IRSensor::readL(){
00048     
00049         measL = IrLeft.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
00050         measL = measL * 1000; // Change the value to be in the 0 to 1000 range
00051         disL = PL1*pow(measL,4)+PL2*pow(measL,3)+PL3*pow(measL,2)+PL4*measL+PL5; //disL = f(measL)
00052         
00053         return disL;    
00054 }
00055 
00056 float IRSensor::readF(){
00057     
00058         measF = IrFront.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
00059         measF = measF * 1000; // Change the value to be in the 0 to 1000 range
00060         disF = PF1*pow(measF,4)+PF2*pow(measF,3)+PF3*pow(measF,2)+PF4*measF+PF5; //disF = f(measF)
00061         
00062         return disF;    
00063 }
00064 
00065 // IR Sensor Zusatand
00066 int IRSensor::codeR(){
00067     
00068         if(disR < minIrR) {
00069             IrR = 1;
00070             } else { IrR = 0; }
00071         return IrR;
00072 }
00073 
00074 int IRSensor ::codeL(){
00075     
00076         if(disL < minIrL) {
00077             IrL = 1;
00078             } else { IrL = 0; }
00079         return IrL;     
00080 }
00081 
00082 int IRSensor ::codeF(){
00083     
00084         if(disF < minIrR) {
00085             IrF = 1;
00086             } else { IrF = 0; }
00087         return IrF;
00088 }