Beste Version
Dependencies: mbed
Fork of Roboshark_V8 by
IRSensor.cpp@13:1687f97d4d82, 2018-05-09 (annotated)
- Committer:
- ahlervin
- Date:
- Wed May 09 17:08:24 2018 +0000
- Revision:
- 13:1687f97d4d82
- Parent:
- 12:cd07a80f0a9a
Optimaler Regler und Schnellster Speed
Who changed what in which revision?
User | Revision | Line number | New 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 | 13:1687f97d4d82 | 33 | const float IRSensor :: Period = 0.02; // 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 | } |