Alexander Lüthard
/
Micromouse
Alle Prinf's auskommentiert
Fork of Micromouse by
IRSensor.cpp@3:076dd7ec7eb4, 2018-04-20 (annotated)
- Committer:
- Helvis
- Date:
- Fri Apr 20 13:37:21 2018 +0000
- Revision:
- 3:076dd7ec7eb4
- Parent:
- 1:2b5f79285a3e
- Child:
- 10:84534846e1f1
v0.9.2
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Helvis | 1:2b5f79285a3e | 1 | #include <cmath> |
Helvis | 1:2b5f79285a3e | 2 | #include "IRSensor.h" |
Helvis | 1:2b5f79285a3e | 3 | |
Helvis | 1:2b5f79285a3e | 4 | using namespace std; |
Helvis | 1:2b5f79285a3e | 5 | |
Helvis | 1:2b5f79285a3e | 6 | IRSensor::IRSensor(AnalogIn& distance) : |
Helvis | 1:2b5f79285a3e | 7 | distance(distance){} |
Helvis | 1:2b5f79285a3e | 8 | |
Helvis | 1:2b5f79285a3e | 9 | IRSensor::~IRSensor() {} |
Helvis | 1:2b5f79285a3e | 10 | |
Helvis | 1:2b5f79285a3e | 11 | float IRSensor::readL() { |
Helvis | 1:2b5f79285a3e | 12 | //Links B2 |
Helvis | 1:2b5f79285a3e | 13 | float a1 = -4.474f*pow(10.0f,4.0f); |
Helvis | 1:2b5f79285a3e | 14 | float a2 = 8.415f*pow(10.0f,4.0f); |
Helvis | 1:2b5f79285a3e | 15 | float a3 = -6.279f*pow(10.0f,4.0f); |
Helvis | 1:2b5f79285a3e | 16 | float a4 = 2.36f*pow(10.0f,4.0f); |
Helvis | 1:2b5f79285a3e | 17 | float a5 = -4670.0f; |
Helvis | 1:2b5f79285a3e | 18 | float a6 = 445.1f; |
Helvis | 1:2b5f79285a3e | 19 | |
Helvis | 1:2b5f79285a3e | 20 | |
Helvis | 3:076dd7ec7eb4 | 21 | //float d2 = a1*pow(distance,5) + a2*pow(distance,4) + a3*pow(distance,3) + a4*pow(distance,2) + a5*distance + a6; |
Helvis | 3:076dd7ec7eb4 | 22 | float d2 = 10.2817f*(1/(pow(distance,1.212121f))); |
Helvis | 1:2b5f79285a3e | 23 | |
Helvis | 1:2b5f79285a3e | 24 | return d2; |
Helvis | 1:2b5f79285a3e | 25 | |
Helvis | 1:2b5f79285a3e | 26 | } |
Helvis | 1:2b5f79285a3e | 27 | |
Helvis | 1:2b5f79285a3e | 28 | float IRSensor::readR(){ |
Helvis | 1:2b5f79285a3e | 29 | //Rechts B1 |
Helvis | 1:2b5f79285a3e | 30 | float b1 = -1.079f*pow(10.0f,4.0f); |
Helvis | 1:2b5f79285a3e | 31 | float b2 = 2.404f*pow(10.0f,4.0f); |
Helvis | 1:2b5f79285a3e | 32 | float b3 = -2.179f*pow(10.0f,4.0f); |
Helvis | 1:2b5f79285a3e | 33 | float b4 = 1.012f*pow(10.0f,4.0f); |
Helvis | 1:2b5f79285a3e | 34 | float b5 = -2519.0f; |
Helvis | 1:2b5f79285a3e | 35 | float b6 = 310.8f; |
Helvis | 1:2b5f79285a3e | 36 | |
Helvis | 3:076dd7ec7eb4 | 37 | //float d1 = b1*pow(distance,5) + b2*pow(distance,4) + b3*pow(distance,3) + b4*pow(distance,2) + b5*distance + b6; |
Helvis | 3:076dd7ec7eb4 | 38 | float d1 = 11.6037f*(1/(pow(distance,1.111111f))); |
Helvis | 1:2b5f79285a3e | 39 | |
Helvis | 1:2b5f79285a3e | 40 | return d1; |
Helvis | 1:2b5f79285a3e | 41 | } |
Helvis | 1:2b5f79285a3e | 42 | |
Helvis | 1:2b5f79285a3e | 43 | float IRSensor::readC(){ |
Helvis | 1:2b5f79285a3e | 44 | //Vorne B4 |
Helvis | 3:076dd7ec7eb4 | 45 | double c1 = -2.352f*pow(10.0f,4.0f); |
Helvis | 3:076dd7ec7eb4 | 46 | double c2 = 5.44f*pow(10.0f,4.0f); |
Helvis | 3:076dd7ec7eb4 | 47 | double c3 = -4.955f*pow(10.0f,4.0f); |
Helvis | 3:076dd7ec7eb4 | 48 | double c4 = 2.268f*pow(10.0f,4.0f); |
Helvis | 3:076dd7ec7eb4 | 49 | double c5 = -5538.0f; |
Helvis | 3:076dd7ec7eb4 | 50 | double c6 = 690.5f; |
Helvis | 1:2b5f79285a3e | 51 | |
Helvis | 3:076dd7ec7eb4 | 52 | //double d4 = c1*pow(distance,5) + c2*pow(distance,4) + c3*pow(distance,3) + c4*pow(distance,2) + c5*distance + c6; |
Helvis | 3:076dd7ec7eb4 | 53 | //float d4 = distance; |
Helvis | 3:076dd7ec7eb4 | 54 | float d4 = 28.053f*(1.0f/distance); |
Helvis | 1:2b5f79285a3e | 55 | |
Helvis | 1:2b5f79285a3e | 56 | return d4; |
Helvis | 1:2b5f79285a3e | 57 | } |
Helvis | 1:2b5f79285a3e | 58 | |
Helvis | 1:2b5f79285a3e | 59 | //float IRSensor::readB(){} |