IRSensor.cpp

00001 /*Roboshark V7
00002 IRSensor.cpp
00003 Erstellt: V. Ahlers
00004 geändert: V.Ahlers
00005 V.5.18
00006 Einlesen und Codieren der IR Sensoren
00007 */
00008 
00009 
00010 #include <cmath>
00011 #include "IRSensor.h"
00012 
00013 using namespace std;
00014 
00015     const float IRSensor :: PR1 = 3.4734*0.000000001;    //Koeffizienten
00016     const float IRSensor :: PR2 = -7.1846*0.000001;
00017     const float IRSensor :: PR3 = 0.0055;
00018     const float IRSensor :: PR4 = -1.9304;
00019     const float IRSensor :: PR5 = 301.2428;
00020     const float IRSensor :: PL1 = 3.4734*0.000000001;
00021     const float IRSensor :: PL2 = -7.1846*0.000001;
00022     const float IRSensor :: PL3 = 0.0055;
00023     const float IRSensor :: PL4 = -1.9304;
00024     const float IRSensor :: PL5 = 301.2428;
00025     const float IRSensor :: PF1 = 6.1767f*pow(10.0f,-10);
00026     const float IRSensor :: PF2 = -1.9975f*pow(10.0f,-6);
00027     const float IRSensor :: PF3 = 0.0024f;
00028     const float IRSensor :: PF4 = -1.3299f;
00029     const float IRSensor :: PF5 = 351.1557f;
00030     const int IRSensor :: minIrR = 88;         // minimal Distanzen
00031     const int IRSensor :: minIrL = 88;
00032     const int IRSensor :: minIrF = 79;
00033     const float IRSensor :: Period = 0.2;       // Ticker Periode
00034   
00035     
00036 
00037 
00038 
00039 
00040 IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F, AnalogIn& LineSensor) : 
00041 IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F), LineSensor(LineSensor){
00042     ticker.attach(callback(this, &IRSensor::codeB), Period);
00043       this-> ende = ende;
00044       ende = 0;
00045     }
00046 
00047 
00048                      
00049 IRSensor::~IRSensor(){
00050     ticker.detach();
00051     }
00052 
00053 
00054 // IR Sensor Distanz in mm
00055 float IRSensor::readR() {
00056     
00057         measR = IrRight.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
00058         measR = measR * 1000; // Change the value to be in the 0 to 1000 range
00059         disR = PR1*pow(measR,4)+PR2*pow(measR,3)+PR3*pow(measR,2)+PR4*measR+PR5; //disR = f(measR)
00060         roundR = disR*100;
00061         disR = roundR/100.0-10.0;
00062         
00063         return disR;    
00064 }
00065 
00066 float IRSensor::readL(){
00067     
00068         measL = IrLeft.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
00069         measL = measL * 1000; // Change the value to be in the 0 to 1000 range
00070         disL = PL1*pow(measL,4)+PL2*pow(measL,3)+PL3*pow(measL,2)+PL4*measL+PL5; //disL = f(measL)
00071         roundL = disL*100;
00072         disL = roundL/100.0-10.0;
00073         
00074         return disL;    
00075 }
00076 
00077 float IRSensor::readF(){
00078     
00079         measF = IrFront.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
00080         measF = measF * 1000; // Change the value to be in the 0 to 1000 range
00081         disF = PF1*pow(measF,4)+PF2*pow(measF,3)+PF3*pow(measF,2)+PF4*measF+PF5; //disF = f(measF)
00082         roundF = disF*100;
00083         disF = roundF/100.0-20.0;
00084         
00085         return disF;    
00086 }
00087 
00088 // IR Sensor Zusatand codieren
00089 int IRSensor::codeR(){
00090     
00091         if(disR < minIrR) {
00092             IrR = 1;
00093             } else { IrR = 0; }
00094         return IrR;
00095 }
00096 
00097 int IRSensor ::codeL(){
00098     
00099         if(disL < minIrL) {
00100             IrL = 1;
00101             } else { IrL = 0; }
00102         return IrL;     
00103 }
00104 
00105 int IRSensor ::codeF(){
00106     
00107         if(disF < minIrF) {
00108             IrF = 1;
00109             } else { IrF = 0; }
00110         return IrF;
00111 }
00112 
00113 //Line Sensor
00114 void IRSensor :: codeB() {
00115     
00116         double line = LineSensor.read();
00117         if (line >0.3){            
00118         finish = 1;
00119          }else{ finish = 0;
00120 }    
00121         if (finish != finishLast){
00122         ende = 1;
00123 }
00124         if (line == 1) {
00125         ende = 1;
00126 }
00127         finishLast = finish;    
00128     return;
00129 }
00130 
00131 int IRSensor::get_ende(){
00132     return ende;
00133     }