edge_func.cpp

00001 /**
00002 *Funktionen zum Auslesen der Linienkamera und zur Detektion der Kante
00003 *
00004 *CPP-Datei edge_func.cpp
00005 *
00006 *@version: 01.11.2015
00007 *@author: Gustav Grether
00008 */
00009 
00010 #include "edge_func.h"
00011 
00012 //Definiton zur Anzeige von Fehlern
00013 DigitalOut errorLED(LED1); //zeigt an, dass Fehler auftritt
00014 
00015 // Definition der Pins des Sensors
00016 DigitalOut clk(p18); //Clock Pin
00017 DigitalOut si(p19); //SI Pin
00018 //analoger Eingang Wertebereich [0..1] mit ao.read()
00019 // misst die Spannung die am Sensorausgang anliegt
00020 AnalogIn ao(p20); 
00021 
00022 //Variablen fuer alle Funktionen der Datei
00023 const int numPx =128; // Anzahl der Pixel
00024 const int leftbound = 8; // linker Randbereich des Sensors, nicht betrachtet
00025 const int rightbound = numPx - leftbound; //rechter Randbereich des Sensors, nicht betrachtet
00026 queue<int> possEdgePx; //enthaelt die Pixel die als Kante in Frage kommen
00027 int tmpPossEdgePx; //temporaeres moegliches Kantenpixel
00028 int numPossEdgePx; //Anzahl der moeglichen Kantenpixel
00029 
00030 //Variablen fuer read()
00031 float currentVal[numPx]; //Werte der Pixel einer Messung
00032 const int numMeas =3; //Werte werden ueber numMeas Messungen gemittelt
00033 float meanVal[numPx]; //Mittelwerte jedes Pixels ueber die (NumMeas) Messungen
00034 float currentPx;
00035 int delay=2; //Verzoegerung zum Einstellen der Belichtungszeit in [10⁻³ s]
00036 //Variablen zur Detektion einer Fehlfunktion des Sensors
00037 const float epsilon=0.01; //minimal zulaessiger Wert eines Pixels
00038 int cntZero; //Anzahl ungueltiger Pixel(<epsilion)
00039 const int zeroMax=5; //maximale Anzahl ungueltiger Pixel pro Messung
00040 bool validMeas; //zeigt ob eine Messungen gueltig ist
00041 
00042 //Variablen fuer gaussian_calc()
00043 float gaussian[numPx]; //gauss gefilterte Grauwerte
00044 
00045 //Variablen fuer gradient_calc()
00046 float gradient[numPx]; //Gradienten der Pixel
00047 
00048 //Variablen fuer nonMaxSupr()
00049 //Gradienten liegt in [-1.0,1.0]
00050 // relevant ist nur [-1.0,0.0]
00051 // Anzahl der Quanten in Intervall=numQuant+1 
00052 const int numQuant=15;
00053 float quantStepSize=1.0/numQuant; // Schrittweite eines Quants
00054 float quantStep[numQuant]; //Quantenstufen
00055 float quant[numPx]; //quantisierte Werte des Gradienten
00056 
00057 //Variablen fuer plausible()
00058 float meanLeft; //Mittelwert der Pixel links eines moeglichen Kantenpixels
00059 float meanRight; //Mittelwert der Pixel rechts eines moeglichen Kantenpixels
00060 const float threshBlack=0.4; //Grenzwert definiert Beschichtung
00061 const float threshFoil=0.7; //Grenzwert definiert Metallfolie
00062 
00063 //Variablen fuer edge_calc()
00064 float gradmin; //minimaler Gradient
00065 int edgePx; //gefundenes Kantenpixel Wertebereich [leftbound..rightbound]
00066 
00067 void clockPulse()
00068 {
00069     clk=1;
00070     clk=0;
00071 }
00072 
00073 void siClkPulse()
00074 {
00075     si=1;
00076     clockPulse();
00077     si=0;
00078 }
00079 
00080 void sens_setup()
00081 {
00082     errorLED=0;
00083     edgePx=-1;
00084     clk=0;
00085     si=0;
00086 
00087     //linker und rechter Randbereich werden nicht betrachtet
00088     for (int i = 0; i < leftbound - 1; i++) {
00089         gaussian[i]= 0.0;
00090         gradient[i]= 0.0;
00091         quant[i]= 0.0;
00092     }
00093     for (int i = rightbound + 1; i < numPx; i++) {
00094         gaussian[i]= 0.0;
00095         gradient[i]= 0.0;
00096         quant[i]= 0.0;
00097     }
00098 
00099     //QuantenStufen zur Quantisierung des Gradienten
00100     for(int k=0; k<numQuant; k++) {
00101         quantStep[k]=-(k+1)*quantStepSize;
00102     }
00103 }
00104 
00105 void read()
00106 {
00107     
00108     edgePx=-10; //Fehler in Algorithmus wenn Wert -10 bleibt
00109     for(int j=0; j<numMeas; j++) {
00110         validMeas=1; // setze Messung gueltig
00111         cntZero=0;  // setze Anzahl ungueltiger Pixel=0
00112         
00113         siClkPulse(); //stoppt Belichtung der Pixel und startet sie erneut
00114         for(int k = 0; k < 130; k++) {
00115             //neue Belichtung der Pixel beginnt bei k=18
00116             //jeder clock-Puls eliminiert den alten Wert eines Pixels
00117             clockPulse();
00118         }
00119 
00120         wait_ms(delay); // Verzoegerung um die Belichtungszeit zu vergroessern
00121         siClkPulse(); //stoppt Belichtung der Pixel und startet sie erneut
00122 
00123         //Auslesen und Speichern der Werte der Pixel
00124         //jeder Clock Puls legt an ao den Wert(Spannung) des naechsten Pixels an
00125         for(int i=0; i < numPx; i++) {
00126             wait_us(5);//Verzoegerung um den analogen Ausgang des Sensors zu stabilisieren
00127             currentPx= ao.read(); //einlesen des Wertes
00128             if(currentPx < epsilon) {
00129                 cntZero++; //zaehlen der ungueltigen Pixel
00130             }
00131             currentVal[i]=currentPx;
00132             clockPulse();
00133 
00134             //ueberpruefen ob Messung noch gueltig ist
00135             if (cntZero > zeroMax) {
00136                 errorLED=!errorLED;
00137                 edgePx= -1; //Fehler Nummer 1
00138                 validMeas=0; // setze Messung fehlerhaft
00139                 break; //stoppt Auslesen des Sensors, startet neue Belichtung
00140             }
00141         }
00142 
00143         //Mittelung der Werte mit vorherigen Messungen wenn neue Messung gueltig
00144         if(validMeas) {
00145             if(j>0) {
00146                 for(int i=0; i<numPx; i++) {
00147                     meanVal[i]=(meanVal[i]+currentVal[i])/2;
00148                 }
00149             } else { //j==0
00150                 for(int i=0; i<numPx; i++) {
00151                     meanVal[i]=currentVal[i];
00152                 }
00153             }
00154         }
00155     }
00156 }
00157 
00158 void gaussian_calc()
00159 {
00160     //Gauß Filter sigma=2, k=5, G(x,sigma)=1/113*[1,3,7,14,20,23,20,14,7,3,1]
00161     for (int i = leftbound - 1; i < rightbound + 1; i++) {
00162         gaussian[i] = (meanVal[i-5]+meanVal[i-4]*3
00163                        +meanVal[i-3]*7+meanVal[i-2]*14
00164                        + meanVal[i-1]*20+meanVal[i]*23
00165                        + meanVal[i+1]*20+meanVal[i+2]*14
00166                        + meanVal[i+3]*7+meanVal[i+4]*3+
00167                        meanVal[i+5])/113;
00168     }
00169 
00170 }
00171 
00172 void gradient_calc()
00173 {
00174     for (int i = leftbound; i < rightbound; i++) {
00175         gradient[i] = (meanVal[i-1]- meanVal[i+1]) / 2;
00176     }
00177 
00178 }
00179 
00180 
00181 void nonMaxSupr()
00182 {
00183     //Quantisierung des Gradienten
00184     for(int i=leftbound; i<rightbound; i++) {
00185         if(gradient[i]> quantStep[0]) {
00186             quant[i]=0.0;//keine Kante
00187         } else {
00188             for(int k=1; k<numQuant; k++) {
00189                 if(gradient[i]> quantStep[k]) {
00190                     quant[i]=quantStep[k-1];
00191                     possEdgePx.push(i); //moegliche Kante
00192                     break;
00193                 }
00194             }
00195         }
00196     }
00197 
00198     //Non-Maxima-Supression
00199 
00200     numPossEdgePx=possEdgePx.size();
00201 
00202     for (int i=0; i<numPossEdgePx; i++) {
00203         tmpPossEdgePx=possEdgePx.front();
00204         possEdgePx.pop();
00205         //Bedingung fuer Kante if(quant[tmpPossEdgePx]<=quant[tmpPossEdgePx-1] && quant[tmpPossEdgePx]<=quant[tmpPossEdgePx+1] )
00206         //negiert damit Gleichheit zwischen Float nicht ueberprüft werden muss
00207         if(!(quant[tmpPossEdgePx]>quant[tmpPossEdgePx-1] || quant[tmpPossEdgePx]>quant[tmpPossEdgePx+1]))  {
00208             possEdgePx.push(tmpPossEdgePx); //moegliche Kante
00209         }
00210     }
00211 }
00212 
00213 void plausible()
00214 {
00215 //Plausibilitaetspruefung
00216     if(possEdgePx.empty()) {
00217         errorLED=!errorLED;
00218         edgePx=-2; // Fehler Nummer 2
00219     }
00220 
00221     //Plausibilitaetspruefung: ueberpruefe die Umgebung jedes moeglichen Kantenpixels
00222     // die Pixel links davon muessen im Mittelwert < threshblack
00223     // die Pixel rechts davon >threshfoil
00224     numPossEdgePx=possEdgePx.size();
00225 
00226     for (int j=0; j<numPossEdgePx; j++) {
00227         tmpPossEdgePx=possEdgePx.front();
00228         possEdgePx.pop();
00229         meanLeft=0.0;
00230         meanRight=0.0;
00231         for(int i=1; i<6; i++) {
00232             meanLeft+=meanVal[tmpPossEdgePx-i];
00233             meanRight+=meanVal[tmpPossEdgePx+i];
00234         }
00235         meanLeft=meanLeft/5;
00236         meanRight=meanRight/5;
00237 
00238         if(meanLeft < threshBlack && meanRight> threshFoil) {
00239             possEdgePx.push(tmpPossEdgePx);
00240         }
00241     }
00242 
00243 }
00244 
00245 void edgePx_calc()
00246 {
00247     if(possEdgePx.empty()) {
00248         errorLED=!errorLED;
00249         edgePx=-3; //Fehler Nummer 3
00250     } else if (possEdgePx.size()==1) {
00251         //nur ein moegliches Kantenpixel uebrig
00252         edgePx=possEdgePx.front();
00253         possEdgePx.pop();
00254     } else {
00255         //mehrere moegliche Kantenpixel;
00256         //bei richtig eingestelltem Sensor sollten hier durch die Plausibilitaetspruefung
00257         //nur noch Nachbarpixel mit gleich großem quantisiertem Gradient uebrig sein
00258         //waehle Pixel mit minimalem Gradient(float)
00259         gradmin = 0.0;
00260         numPossEdgePx = possEdgePx.size();
00261         for (int i = 0; i < numPossEdgePx; i++) {
00262             tmpPossEdgePx=possEdgePx.front();
00263             possEdgePx.pop();
00264             if ( gradient[tmpPossEdgePx]< gradmin) {
00265                 gradmin=gradient[tmpPossEdgePx];
00266                 edgePx=tmpPossEdgePx;//gefundenes Kantenpixel, Algorithmus erfolgreich
00267                 errorLED=0;
00268             }
00269         }
00270     }
00271 }
00272 
00273 int edgePx_get()
00274 {
00275     read();
00276     gaussian_calc();
00277     gradient_calc();
00278     nonMaxSupr();
00279     plausible();
00280     edgePx_calc();
00281     return edgePx;
00282 }
00283