Roboshark / Mbed 2 deprecated Main_1

Sensoren auslesen/umwandeln/codieren und State Maschine

Dependencies:   mbed

Fork of StateMachine_1 by Roboshark

IRSensor.cpp@4:91a9737e4821, 2018-04-20 (annotated)

Committer:
ahlervin
Date:
Fri Apr 20 17:49:36 2018 +0000
Revision:
4:91a9737e4821
Sensoren auswerten, Codieren und State Machine

Who changed what in which revision?

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