Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

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
Child:
21:41997651337a
v0.9.2

Who changed what in which revision?

UserRevisionLine numberNew 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(){}