Helvijs Kiselis
/
MicromousePES2
main.cpp: Sensoren einlesen und Motoren ansteuern
Diff: IRSensor.cpp
- Revision:
- 5:47262622a9bf
- Parent:
- 1:1adf5dfcc7bb
--- a/IRSensor.cpp Wed Apr 11 15:26:03 2018 +0000 +++ b/IRSensor.cpp Mon Apr 16 12:44:48 2018 +0000 @@ -8,11 +8,48 @@ IRSensor::~IRSensor() {} -float IRSensor::read() { +float IRSensor::readL() { + //Links B2 + float a1 = -4.474f*pow(10.0f,4.0f); + float a2 = 8.415f*pow(10.0f,4.0f); + float a3 = -6.279f*pow(10.0f,4.0f); + float a4 = 2.36f*pow(10.0f,4.0f); + float a5 = -4670.0f; + float a6 = 445.1f; + + + float d2 = a1*pow(distance,5) + a2*pow(distance,4) + a3*pow(distance,3) + a4*pow(distance,2) + a5*distance + a6; + + return d2; - float d = -0.58f*sqrt(distance)+0.58f; - //float d = distance; +} + +float IRSensor::readR(){ + //Rechts B1 + float b1 = -1.079f*pow(10.0f,4.0f); + float b2 = 2.404f*pow(10.0f,4.0f); + float b3 = -2.179f*pow(10.0f,4.0f); + float b4 = 1.012f*pow(10.0f,4.0f); + float b5 = -2519.0f; + float b6 = 310.8f; + + float d1 = b1*pow(distance,5) + b2*pow(distance,4) + b3*pow(distance,3) + b4*pow(distance,2) + b5*distance + b6; - return d; + return d1; +} + +float IRSensor::readC(){ + //Vorne B4 + float c1 = -2.681f*pow(10.0f,4.0f); + float c2 = 6.007f*pow(10.0f,4.0f); + float c3 = -5.32f*pow(10.0f,4.0f); + float c4 = 2.377f*pow(10.0f,4.0f); + float c5 = -5686.0f; + float c6 = 697.8f; -} \ No newline at end of file + float d4 = c1*pow(distance,5) + c2*pow(distance,4) + c3*pow(distance,3) + c4*pow(distance,2) + c5*distance + c6; + + return d4; + } + +//float IRSensor::readB(){} \ No newline at end of file