Nicht funktionierender Regler
Dependencies: mbed
Fork of Roboshark_V3 by
Diff: IRSensor.cpp
- Revision:
- 0:6d0671ae4648
- Child:
- 4:767fd282dd9c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IRSensor.cpp Mon Apr 23 11:28:11 2018 +0000 @@ -0,0 +1,88 @@ + +//Implementation IR Sensoren +// V04.18 +// V. Ahlers + +#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 = 100; //Noch definieren + const int IRSensor :: minIrL = 100; + const int IRSensor :: minIrF = 100; + + + +// IR Sensor Distanz in mm +IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F) : +IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F){} + +IRSensor::~IRSensor(){} + +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) + + 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) + + 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) + + return disF; +} + +// IR Sensor Zusatand +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 < minIrR) { + IrF = 1; + } else { IrF = 0; } + return IrF; +} \ No newline at end of file