Roboshark / Mbed 2 deprecated Roboshark_V9

Beste Version

Dependencies:   mbed

Fork of Roboshark_V8 by Roboshark

IRSensor.cpp@12:cd07a80f0a9a, 2018-05-08 (annotated)

Committer:
ahlervin
Date:
Tue May 08 10:48:05 2018 +0000
Revision:
12:cd07a80f0a9a
Parent:
9:feabe0b7cea4
Child:
13:1687f97d4d82
Beste Version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ahlervin 9:feabe0b7cea4 1 /*Roboshark V7
ahlervin 6:7bbcdd07bc2d 2 IRSensor.cpp
ahlervin 6:7bbcdd07bc2d 3 Erstellt: V. Ahlers
ahlervin 6:7bbcdd07bc2d 4 geändert: V.Ahlers
ahlervin 6:7bbcdd07bc2d 5 V.5.18
ahlervin 6:7bbcdd07bc2d 6 Einlesen und Codieren der IR Sensoren
ahlervin 6:7bbcdd07bc2d 7 */
Jacqueline 0:6d0671ae4648 8
Jacqueline 0:6d0671ae4648 9
Jacqueline 0:6d0671ae4648 10 #include <cmath>
Jacqueline 0:6d0671ae4648 11 #include "IRSensor.h"
Jacqueline 0:6d0671ae4648 12
Jacqueline 0:6d0671ae4648 13 using namespace std;
Jacqueline 0:6d0671ae4648 14
Jacqueline 0:6d0671ae4648 15 const float IRSensor :: PR1 = 3.4734*0.000000001; //Koeffizienten
Jacqueline 0:6d0671ae4648 16 const float IRSensor :: PR2 = -7.1846*0.000001;
Jacqueline 0:6d0671ae4648 17 const float IRSensor :: PR3 = 0.0055;
Jacqueline 0:6d0671ae4648 18 const float IRSensor :: PR4 = -1.9304;
Jacqueline 0:6d0671ae4648 19 const float IRSensor :: PR5 = 301.2428;
Jacqueline 0:6d0671ae4648 20 const float IRSensor :: PL1 = 3.4734*0.000000001;
Jacqueline 0:6d0671ae4648 21 const float IRSensor :: PL2 = -7.1846*0.000001;
Jacqueline 0:6d0671ae4648 22 const float IRSensor :: PL3 = 0.0055;
Jacqueline 0:6d0671ae4648 23 const float IRSensor :: PL4 = -1.9304;
Jacqueline 0:6d0671ae4648 24 const float IRSensor :: PL5 = 301.2428;
Jacqueline 0:6d0671ae4648 25 const float IRSensor :: PF1 = 6.1767f*pow(10.0f,-10);
Jacqueline 0:6d0671ae4648 26 const float IRSensor :: PF2 = -1.9975f*pow(10.0f,-6);
Jacqueline 0:6d0671ae4648 27 const float IRSensor :: PF3 = 0.0024f;
Jacqueline 0:6d0671ae4648 28 const float IRSensor :: PF4 = -1.3299f;
Jacqueline 0:6d0671ae4648 29 const float IRSensor :: PF5 = 351.1557f;
ahlervin 7:862d80e0ea2d 30 const int IRSensor :: minIrR = 88; // minimal Distanzen
ahlervin 7:862d80e0ea2d 31 const int IRSensor :: minIrL = 88;
ahlervin 7:862d80e0ea2d 32 const int IRSensor :: minIrF = 79;
ahlervin 12:cd07a80f0a9a 33 const float IRSensor :: Period = 0.05; // Ticker Periode
ahlervin 4:767fd282dd9c 34
ahlervin 4:767fd282dd9c 35
Jacqueline 0:6d0671ae4648 36
Jacqueline 0:6d0671ae4648 37
Jacqueline 0:6d0671ae4648 38
ahlervin 6:7bbcdd07bc2d 39
ahlervin 4:767fd282dd9c 40 IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F, AnalogIn& LineSensor) :
ahlervin 4:767fd282dd9c 41 IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F), LineSensor(LineSensor){
ahlervin 4:767fd282dd9c 42 ticker.attach(callback(this, &IRSensor::codeB), Period);
ahlervin 4:767fd282dd9c 43 this-> ende = ende;
ahlervin 4:767fd282dd9c 44 ende = 0;
ahlervin 4:767fd282dd9c 45 }
ahlervin 4:767fd282dd9c 46
ahlervin 4:767fd282dd9c 47
Jacqueline 0:6d0671ae4648 48
ahlervin 4:767fd282dd9c 49 IRSensor::~IRSensor(){
ahlervin 4:767fd282dd9c 50 ticker.detach();
ahlervin 4:767fd282dd9c 51 }
ahlervin 4:767fd282dd9c 52
ahlervin 4:767fd282dd9c 53
ahlervin 6:7bbcdd07bc2d 54 // IR Sensor Distanz in mm
Jacqueline 0:6d0671ae4648 55 float IRSensor::readR() {
Jacqueline 0:6d0671ae4648 56
Jacqueline 0:6d0671ae4648 57 measR = IrRight.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Jacqueline 0:6d0671ae4648 58 measR = measR * 1000; // Change the value to be in the 0 to 1000 range
Jacqueline 0:6d0671ae4648 59 disR = PR1*pow(measR,4)+PR2*pow(measR,3)+PR3*pow(measR,2)+PR4*measR+PR5; //disR = f(measR)
ahlervin 7:862d80e0ea2d 60 roundR = disR*100;
ahlervin 7:862d80e0ea2d 61 disR = roundR/100.0-10.0;
Jacqueline 0:6d0671ae4648 62
Jacqueline 0:6d0671ae4648 63 return disR;
Jacqueline 0:6d0671ae4648 64 }
Jacqueline 0:6d0671ae4648 65
Jacqueline 0:6d0671ae4648 66 float IRSensor::readL(){
Jacqueline 0:6d0671ae4648 67
Jacqueline 0:6d0671ae4648 68 measL = IrLeft.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Jacqueline 0:6d0671ae4648 69 measL = measL * 1000; // Change the value to be in the 0 to 1000 range
Jacqueline 0:6d0671ae4648 70 disL = PL1*pow(measL,4)+PL2*pow(measL,3)+PL3*pow(measL,2)+PL4*measL+PL5; //disL = f(measL)
ahlervin 7:862d80e0ea2d 71 roundL = disL*100;
ahlervin 7:862d80e0ea2d 72 disL = roundL/100.0-10.0;
Jacqueline 0:6d0671ae4648 73
Jacqueline 0:6d0671ae4648 74 return disL;
Jacqueline 0:6d0671ae4648 75 }
Jacqueline 0:6d0671ae4648 76
Jacqueline 0:6d0671ae4648 77 float IRSensor::readF(){
Jacqueline 0:6d0671ae4648 78
Jacqueline 0:6d0671ae4648 79 measF = IrFront.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Jacqueline 0:6d0671ae4648 80 measF = measF * 1000; // Change the value to be in the 0 to 1000 range
Jacqueline 0:6d0671ae4648 81 disF = PF1*pow(measF,4)+PF2*pow(measF,3)+PF3*pow(measF,2)+PF4*measF+PF5; //disF = f(measF)
ahlervin 7:862d80e0ea2d 82 roundF = disF*100;
ahlervin 7:862d80e0ea2d 83 disF = roundF/100.0-20.0;
Jacqueline 0:6d0671ae4648 84
Jacqueline 0:6d0671ae4648 85 return disF;
Jacqueline 0:6d0671ae4648 86 }
Jacqueline 0:6d0671ae4648 87
ahlervin 6:7bbcdd07bc2d 88 // IR Sensor Zusatand codieren
Jacqueline 0:6d0671ae4648 89 int IRSensor::codeR(){
Jacqueline 0:6d0671ae4648 90
Jacqueline 0:6d0671ae4648 91 if(disR < minIrR) {
Jacqueline 0:6d0671ae4648 92 IrR = 1;
Jacqueline 0:6d0671ae4648 93 } else { IrR = 0; }
Jacqueline 0:6d0671ae4648 94 return IrR;
Jacqueline 0:6d0671ae4648 95 }
Jacqueline 0:6d0671ae4648 96
Jacqueline 0:6d0671ae4648 97 int IRSensor ::codeL(){
Jacqueline 0:6d0671ae4648 98
Jacqueline 0:6d0671ae4648 99 if(disL < minIrL) {
Jacqueline 0:6d0671ae4648 100 IrL = 1;
Jacqueline 0:6d0671ae4648 101 } else { IrL = 0; }
Jacqueline 0:6d0671ae4648 102 return IrL;
Jacqueline 0:6d0671ae4648 103 }
Jacqueline 0:6d0671ae4648 104
Jacqueline 0:6d0671ae4648 105 int IRSensor ::codeF(){
Jacqueline 0:6d0671ae4648 106
ahlervin 5:e715d157ced5 107 if(disF < minIrF) {
Jacqueline 0:6d0671ae4648 108 IrF = 1;
Jacqueline 0:6d0671ae4648 109 } else { IrF = 0; }
ahlervin 9:feabe0b7cea4 110 return IrF;
ahlervin 4:767fd282dd9c 111 }
ahlervin 4:767fd282dd9c 112
ahlervin 6:7bbcdd07bc2d 113 //Line Sensor
ahlervin 4:767fd282dd9c 114 void IRSensor :: codeB() {
ahlervin 4:767fd282dd9c 115
ahlervin 4:767fd282dd9c 116 double line = LineSensor.read();
ahlervin 6:7bbcdd07bc2d 117 if (line >0.3){
ahlervin 4:767fd282dd9c 118 finish = 1;
ahlervin 4:767fd282dd9c 119 }else{ finish = 0;
ahlervin 4:767fd282dd9c 120 }
ahlervin 4:767fd282dd9c 121 if (finish != finishLast){
ahlervin 4:767fd282dd9c 122 ende = 1;
ahlervin 4:767fd282dd9c 123 }
ahlervin 4:767fd282dd9c 124 if (line == 1) {
ahlervin 4:767fd282dd9c 125 ende = 1;
ahlervin 4:767fd282dd9c 126 }
ahlervin 4:767fd282dd9c 127 finishLast = finish;
ahlervin 4:767fd282dd9c 128 return;
ahlervin 4:767fd282dd9c 129 }
ahlervin 4:767fd282dd9c 130
ahlervin 4:767fd282dd9c 131 int IRSensor::get_ende(){
ahlervin 4:767fd282dd9c 132 return ende;
ahlervin 4:767fd282dd9c 133 }