Nicht funktionierender Regler
Dependencies: mbed
Fork of Roboshark_V3 by
IRSensor.cpp
- Committer:
- ahlervin
- Date:
- 2018-05-02
- Revision:
- 8:d7dfee648545
- Parent:
- 5:e715d157ced5
File content as of revision 8:d7dfee648545:
//Implementation IR Sensoren // V04.18 // V. Ahlers #include <cmath> #include "IRSensor.h" using namespace std; const float IRSensor :: PR1 = 3.4734*0.000000001; //Koeffizienten const float IRSensor :: PR2 = -7.1846*0.000001; const float IRSensor :: PR3 = 0.0055; const float IRSensor :: PR4 = -1.9304; const float IRSensor :: PR5 = 301.2428; const float IRSensor :: PL1 = 3.4734*0.000000001; const float IRSensor :: PL2 = -7.1846*0.000001; const float IRSensor :: PL3 = 0.0055; const float IRSensor :: PL4 = -1.9304; const float IRSensor :: PL5 = 301.2428; const float IRSensor :: PF1 = 6.1767f*pow(10.0f,-10); const float IRSensor :: PF2 = -1.9975f*pow(10.0f,-6); const float IRSensor :: PF3 = 0.0024f; const float IRSensor :: PF4 = -1.3299f; const float IRSensor :: PF5 = 351.1557f; const int IRSensor :: minIrR = 100; //Noch definieren const int IRSensor :: minIrL = 100; const int IRSensor :: minIrF = 100; const float IRSensor :: Period = 0.2; // IR Sensor Distanz in mm IRSensor::IRSensor(AnalogIn& IrRight, AnalogIn& IrLeft, AnalogIn& IrFront, float dis2R, float dis2L, float dis2F, AnalogIn& LineSensor) : IrRight(IrRight), IrLeft(IrLeft), IrFront(IrFront), dis2R(dis2R), dis2L(dis2L), dis2F(dis2F), LineSensor(LineSensor){ ticker.attach(callback(this, &IRSensor::codeB), Period); this-> ende = ende; ende = 0; } IRSensor::~IRSensor(){ ticker.detach(); } float IRSensor::readR() { measR = IrRight.read(); // Converts and read the analog input value (value from 0.0 to 1.0) measR = measR * 1000; // Change the value to be in the 0 to 1000 range disR = PR1*pow(measR,4)+PR2*pow(measR,3)+PR3*pow(measR,2)+PR4*measR+PR5; //disR = f(measR) return disR; } float IRSensor::readL(){ measL = IrLeft.read(); // Converts and read the analog input value (value from 0.0 to 1.0) measL = measL * 1000; // Change the value to be in the 0 to 1000 range disL = PL1*pow(measL,4)+PL2*pow(measL,3)+PL3*pow(measL,2)+PL4*measL+PL5; //disL = f(measL) return disL; } float IRSensor::readF(){ measF = IrFront.read(); // Converts and read the analog input value (value from 0.0 to 1.0) measF = measF * 1000; // Change the value to be in the 0 to 1000 range disF = PF1*pow(measF,4)+PF2*pow(measF,3)+PF3*pow(measF,2)+PF4*measF+PF5; //disF = f(measF) return disF; } // IR Sensor Zusatand int IRSensor::codeR(){ if(disR < minIrR) { IrR = 1; } else { IrR = 0; } return IrR; } int IRSensor ::codeL(){ if(disL < minIrL) { IrL = 1; } else { IrL = 0; } return IrL; } int IRSensor ::codeF(){ if(disF < minIrF) { IrF = 1; } else { IrF = 0; } return IrF;//IrF; } void IRSensor :: codeB() { double line = LineSensor.read(); if (line >0.3){ //Line Sensor finish = 1; }else{ finish = 0; } if (finish != finishLast){ ende = 1; } if (line == 1) { ende = 1; } finishLast = finish; //printf("Line Sensor ist %d\n",ende); return; } int IRSensor::get_ende(){ return ende; }