Beste Version
Dependencies: mbed
Fork of Roboshark_V8 by
IRSensor.cpp
- Committer:
- ahlervin
- Date:
- 2018-05-09
- Revision:
- 13:1687f97d4d82
- Parent:
- 12:cd07a80f0a9a
File content as of revision 13:1687f97d4d82:
/*Roboshark V7 IRSensor.cpp Erstellt: V. Ahlers geändert: V.Ahlers V.5.18 Einlesen und Codieren der IR Sensoren */ #include <cmath> #include "IRSensor.h" using namespace std; const float IRSensor :: PR1 = 3.4734*0.000000001; //Koeffizienten const float IRSensor :: PR2 = -7.1846*0.000001; const float IRSensor :: PR3 = 0.0055; const float IRSensor :: PR4 = -1.9304; const float IRSensor :: PR5 = 301.2428; const float IRSensor :: PL1 = 3.4734*0.000000001; const float IRSensor :: PL2 = -7.1846*0.000001; const float IRSensor :: PL3 = 0.0055; const float IRSensor :: PL4 = -1.9304; const float IRSensor :: PL5 = 301.2428; const float IRSensor :: PF1 = 6.1767f*pow(10.0f,-10); const float IRSensor :: PF2 = -1.9975f*pow(10.0f,-6); const float IRSensor :: PF3 = 0.0024f; const float IRSensor :: PF4 = -1.3299f; const float IRSensor :: PF5 = 351.1557f; const int IRSensor :: minIrR = 88; // minimal Distanzen const int IRSensor :: minIrL = 88; const int IRSensor :: minIrF = 79; const float IRSensor :: Period = 0.02; // Ticker Periode IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F, AnalogIn& LineSensor) : IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F), LineSensor(LineSensor){ ticker.attach(callback(this, &IRSensor::codeB), Period); this-> ende = ende; ende = 0; } IRSensor::~IRSensor(){ ticker.detach(); } // IR Sensor Distanz in mm float IRSensor::readR() { measR = IrRight.read(); // Converts and read the analog input value (value from 0.0 to 1.0) measR = measR * 1000; // Change the value to be in the 0 to 1000 range disR = PR1*pow(measR,4)+PR2*pow(measR,3)+PR3*pow(measR,2)+PR4*measR+PR5; //disR = f(measR) roundR = disR*100; disR = roundR/100.0-10.0; return disR; } float IRSensor::readL(){ measL = IrLeft.read(); // Converts and read the analog input value (value from 0.0 to 1.0) measL = measL * 1000; // Change the value to be in the 0 to 1000 range disL = PL1*pow(measL,4)+PL2*pow(measL,3)+PL3*pow(measL,2)+PL4*measL+PL5; //disL = f(measL) roundL = disL*100; disL = roundL/100.0-10.0; return disL; } float IRSensor::readF(){ measF = IrFront.read(); // Converts and read the analog input value (value from 0.0 to 1.0) measF = measF * 1000; // Change the value to be in the 0 to 1000 range disF = PF1*pow(measF,4)+PF2*pow(measF,3)+PF3*pow(measF,2)+PF4*measF+PF5; //disF = f(measF) roundF = disF*100; disF = roundF/100.0-20.0; return disF; } // IR Sensor Zusatand codieren int IRSensor::codeR(){ if(disR < minIrR) { IrR = 1; } else { IrR = 0; } return IrR; } int IRSensor ::codeL(){ if(disL < minIrL) { IrL = 1; } else { IrL = 0; } return IrL; } int IRSensor ::codeF(){ if(disF < minIrF) { IrF = 1; } else { IrF = 0; } return IrF; } //Line Sensor void IRSensor :: codeB() { double line = LineSensor.read(); if (line >0.3){ finish = 1; }else{ finish = 0; } if (finish != finishLast){ ende = 1; } if (line == 1) { ende = 1; } finishLast = finish; return; } int IRSensor::get_ende(){ return ende; }