Roboshark / Mbed 2 deprecated Roboshark_V2

kann fahren und drehen

Dependencies:   mbed

IRSensor.cpp@3:e4264cb5b9a9, 2018-04-23 (annotated)

Committer:
Jacqueline
Date:
Mon Apr 23 12:19:12 2018 +0000
Revision:
3:e4264cb5b9a9
Parent:
0:6d0671ae4648 
neu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jacqueline 0:6d0671ae4648 1
Jacqueline 0:6d0671ae4648 2 //Implementation IR Sensoren
Jacqueline 0:6d0671ae4648 3 // V04.18
Jacqueline 0:6d0671ae4648 4 // V. Ahlers
Jacqueline 0:6d0671ae4648 5
Jacqueline 0:6d0671ae4648 6 #include <cmath>
Jacqueline 0:6d0671ae4648 7 #include "IRSensor.h"
Jacqueline 0:6d0671ae4648 8
Jacqueline 0:6d0671ae4648 9 using namespace std;
Jacqueline 0:6d0671ae4648 10
Jacqueline 0:6d0671ae4648 11 const float IRSensor :: PR1 = 3.4734*0.000000001; //Koeffizienten
Jacqueline 0:6d0671ae4648 12 const float IRSensor :: PR2 = -7.1846*0.000001;
Jacqueline 0:6d0671ae4648 13 const float IRSensor :: PR3 = 0.0055;
Jacqueline 0:6d0671ae4648 14 const float IRSensor :: PR4 = -1.9304;
Jacqueline 0:6d0671ae4648 15 const float IRSensor :: PR5 = 301.2428;
Jacqueline 0:6d0671ae4648 16 const float IRSensor :: PL1 = 3.4734*0.000000001;
Jacqueline 0:6d0671ae4648 17 const float IRSensor :: PL2 = -7.1846*0.000001;
Jacqueline 0:6d0671ae4648 18 const float IRSensor :: PL3 = 0.0055;
Jacqueline 0:6d0671ae4648 19 const float IRSensor :: PL4 = -1.9304;
Jacqueline 0:6d0671ae4648 20 const float IRSensor :: PL5 = 301.2428;
Jacqueline 0:6d0671ae4648 21 const float IRSensor :: PF1 = 6.1767f*pow(10.0f,-10);
Jacqueline 0:6d0671ae4648 22 const float IRSensor :: PF2 = -1.9975f*pow(10.0f,-6);
Jacqueline 0:6d0671ae4648 23 const float IRSensor :: PF3 = 0.0024f;
Jacqueline 0:6d0671ae4648 24 const float IRSensor :: PF4 = -1.3299f;
Jacqueline 0:6d0671ae4648 25 const float IRSensor :: PF5 = 351.1557f;
Jacqueline 0:6d0671ae4648 26 const int IRSensor :: minIrR = 100; //Noch definieren
Jacqueline 0:6d0671ae4648 27 const int IRSensor :: minIrL = 100;
Jacqueline 0:6d0671ae4648 28 const int IRSensor :: minIrF = 100;
Jacqueline 0:6d0671ae4648 29
Jacqueline 0:6d0671ae4648 30
Jacqueline 0:6d0671ae4648 31
Jacqueline 0:6d0671ae4648 32 // IR Sensor Distanz in mm
Jacqueline 0:6d0671ae4648 33 IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F) :
Jacqueline 0:6d0671ae4648 34 IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F){}
Jacqueline 0:6d0671ae4648 35
Jacqueline 0:6d0671ae4648 36 IRSensor::~IRSensor(){}
Jacqueline 0:6d0671ae4648 37
Jacqueline 0:6d0671ae4648 38 float IRSensor::readR() {
Jacqueline 0:6d0671ae4648 39
Jacqueline 0:6d0671ae4648 40 measR = IrRight.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Jacqueline 0:6d0671ae4648 41 measR = measR * 1000; // Change the value to be in the 0 to 1000 range
Jacqueline 0:6d0671ae4648 42 disR = PR1*pow(measR,4)+PR2*pow(measR,3)+PR3*pow(measR,2)+PR4*measR+PR5; //disR = f(measR)
Jacqueline 0:6d0671ae4648 43
Jacqueline 0:6d0671ae4648 44 return disR;
Jacqueline 0:6d0671ae4648 45 }
Jacqueline 0:6d0671ae4648 46
Jacqueline 0:6d0671ae4648 47 float IRSensor::readL(){
Jacqueline 0:6d0671ae4648 48
Jacqueline 0:6d0671ae4648 49 measL = IrLeft.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Jacqueline 0:6d0671ae4648 50 measL = measL * 1000; // Change the value to be in the 0 to 1000 range
Jacqueline 0:6d0671ae4648 51 disL = PL1*pow(measL,4)+PL2*pow(measL,3)+PL3*pow(measL,2)+PL4*measL+PL5; //disL = f(measL)
Jacqueline 0:6d0671ae4648 52
Jacqueline 0:6d0671ae4648 53 return disL;
Jacqueline 0:6d0671ae4648 54 }
Jacqueline 0:6d0671ae4648 55
Jacqueline 0:6d0671ae4648 56 float IRSensor::readF(){
Jacqueline 0:6d0671ae4648 57
Jacqueline 0:6d0671ae4648 58 measF = IrFront.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Jacqueline 0:6d0671ae4648 59 measF = measF * 1000; // Change the value to be in the 0 to 1000 range
Jacqueline 0:6d0671ae4648 60 disF = PF1*pow(measF,4)+PF2*pow(measF,3)+PF3*pow(measF,2)+PF4*measF+PF5; //disF = f(measF)
Jacqueline 0:6d0671ae4648 61
Jacqueline 0:6d0671ae4648 62 return disF;
Jacqueline 0:6d0671ae4648 63 }
Jacqueline 0:6d0671ae4648 64
Jacqueline 0:6d0671ae4648 65 // IR Sensor Zusatand
Jacqueline 0:6d0671ae4648 66 int IRSensor::codeR(){
Jacqueline 0:6d0671ae4648 67
Jacqueline 0:6d0671ae4648 68 if(disR < minIrR) {
Jacqueline 0:6d0671ae4648 69 IrR = 1;
Jacqueline 0:6d0671ae4648 70 } else { IrR = 0; }
Jacqueline 0:6d0671ae4648 71 return IrR;
Jacqueline 0:6d0671ae4648 72 }
Jacqueline 0:6d0671ae4648 73
Jacqueline 0:6d0671ae4648 74 int IRSensor ::codeL(){
Jacqueline 0:6d0671ae4648 75
Jacqueline 0:6d0671ae4648 76 if(disL < minIrL) {
Jacqueline 0:6d0671ae4648 77 IrL = 1;
Jacqueline 0:6d0671ae4648 78 } else { IrL = 0; }
Jacqueline 0:6d0671ae4648 79 return IrL;
Jacqueline 0:6d0671ae4648 80 }
Jacqueline 0:6d0671ae4648 81
Jacqueline 0:6d0671ae4648 82 int IRSensor ::codeF(){
Jacqueline 0:6d0671ae4648 83
Jacqueline 0:6d0671ae4648 84 if(disF < minIrR) {
Jacqueline 0:6d0671ae4648 85 IrF = 1;
Jacqueline 0:6d0671ae4648 86 } else { IrF = 0; }
Jacqueline 0:6d0671ae4648 87 return IrF;
Jacqueline 0:6d0671ae4648 88 }