Im Labyrinth werden die Linienenden mit verschiedenen Farben markiert.
Das Programmbeispiel ist für nachfolgende 8 Farben angepasst.
Im Programmbeispiel wird beim Drücken der Taste 1 die Startfarbe,
beim Drücken der Taste 2 die Zielfarbe gespeichert.
Beim Drücken der Taste 3 wird die laufende Messung gestartet.
Dann Blinken bei der Startfarbe LED 1+2 und bei der Zielfarbe LED 3+4.
#include <NIBOburger.h>
#include <niboburger\analog.h> // Wird wegen SurfaceSensor.getCL(0); SurfaceSensor.getCR(0); benötigt.
#include <niboburger\odometry.h>
// c, l, r-Werte werden bei der Farberkennung ausgewertet
int c[3] = {0};
int l[3] = {0};
int r[3] = {0};
int cl[3] = {0};
int cr[3] = {0};
// Messwerte
int messwert[5] = {0};
// Toleranzbereich
int cmax,cmin,lmax,lmin,rmax,rmin,clmax,clmin,crmax,crmin;
// Hilfsvariable
int j = 0;
char key = ' ';
void setup ()
{
NIBOburger.begin();
Serial.begin(38400);
analog_setExtToggleMode(ANALOG_BCL, 1);
analog_setExtToggleMode(ANALOG_BCR, 1);
odometry_init();
NIBOburger.setLeds(1,1,0,0);
while (key != 'A')
{
key = NIBOburger.getKeyChar();
}
messen();
NIBOburger.setLeds(0,0,1,1);
while (key != 'B')
{
key = NIBOburger.getKeyChar();
}
messen();
NIBOburger.setLeds(0,0,0,0);
while (key != 'C')
{
key = NIBOburger.getKeyChar();
}
}
void loop ()
{
// messen
messen();
for (int i = 0; i < 3; i++)
{
j = i + 1;
// Toleranzbereich berechnen
if (c[i] < 100)
{
cmin = 0;
cmax = 100;
}
else
{
cmin = c[i] * 0.7;
cmax = c[i] * 1.3;
}
if (l[i] < 100)
{
lmin = 0;
lmax = 100;
}
else
{
lmin = l[i] * 0.7;
lmax = l[i] * 1.3;
}
if (r[i] < 100)
{
rmin = 0;
rmax = 100;
}
else
{
rmin = r[i] * 0.7;
rmax = r[i] * 1.3;
}
// Wird eine Farbe erkannt, wird mit break der loop verlassen
if ((messwert[0] >= cmin) && (messwert[0] < cmax) && (messwert[1] >= lmin) && (messwert[1] < lmax) && (messwert[2] >= rmin) && (messwert[2] < rmax)) break;
} // Loop Ende
// LEDs setzen
if (j > 2) j = 0;
NIBOburger.setLeds(j & 1,0,j & 2,0);
delay(200);
NIBOburger.setLeds(0,j & 1,0,j & 2);
delay(200);
}
void messen()
{
int n = 0;
do
{
messwert[0] = SurfaceSensor.getC(0);
messwert[1] = SurfaceSensor.getL(0);
messwert[2] = SurfaceSensor.getR(0);
messwert[3] = SurfaceSensor.getCL(0);
messwert[4] = SurfaceSensor.getCR(0);
// Etwa aller 100 bis 200 Messungen ist eine einzelne Fehlmessung, bei der alle Messwerte == 0 sind.
// Beim Messen der Farbe Schwarz sind ebenfalls alle Messwerte == 0.
// Bei mehreren aufeinanderfolgenden Messungen (messwert[0] == 0) wird die Farbe Schwarz gemessen.
if (messwert[0] == 0) n++;
else n = 0;
// Ungültige Messwerte (messwert[0] ~ 20000) werden hier gefiltert
} while ((messwert[0] > 1600) || ((messwert[0] == 0) && (n < 4)));
// Laufende Messung
if (key == 'C') ;
// Start-Farbe
else if (key == 'A')
{
c[0] = messwert[0];
l[0] = messwert[1];
r[0] = messwert[2];
cl[0] = messwert[3];
cr[0] = messwert[4];
}
// End-Farbe
else if (key == 'B')
{
c[1] = messwert[0];
l[1] = messwert[1];
r[1] = messwert[2];
cl[1] = messwert[3];
cr[1] = messwert[4];
}
}