Hallo
Es wächst und gedeiht, hier mein einfacher Linienfolger:
Code:
#include "RP6RobotBaseLib.h"
#define mpower 50
#define spur 20
// Achtung! Die PWM-Werte werden hier OHNE Rampe verändert!
void setMotorPWM(uint8_t power_links, uint8_t power_rechts)
{
extern uint8_t mleft_ptmp, mright_ptmp;
if(power_links > 210) power_links = 210;
if(power_rechts > 210) power_rechts = 210;
mleft_power=mleft_ptmp=power_links;
mright_power=mright_ptmp=power_rechts;
OCR1BL = power_links;
OCR1AL = power_rechts;
if(power_links || power_rechts)
TCCR1A = (1 << WGM11) | (1 << COM1A1) | (1 << COM1B1);
else
TCCR1A = 0;
}
uint8_t get_line(uint16_t delay, uint8_t zeilen)
{
uint8_t pixel[256],*pixelzeiger;
uint8_t i, step, lines, h_step, h_sync, h_delay;
uint16_t gamma[16],g_hell, g_dunkel, g_sprung;
uint16_t strich_links, strich_rechts, strich_mitte, strich_breite;
step=(260-35)/zeilen; // 30-260 sind die sichtbare Zeilen
lines=zeilen; // Anzahl der einzulesenden Zeilen
pixelzeiger=&pixel[0]; // Zeiger auf Start Pixelspeicher
cli();
do // vsync abwarten
{
h_sync=0; while (ADCH > 20); while (ADCH < 30) h_sync++;
} while (h_sync < 40);
h_step=35;
while (h_step) // 30 Zeilen Austastzeit überlesen
{
while (ADCH > 20); while (ADCH < 30); h_step--;
}
while (lines--)
{
h_step=step;
while (h_step) // auf die nächste gültige Zeile warten
{
while (ADCH > 20); while (ADCH < 30); h_step--;
}
h_delay=delay;
while (h_delay--); // auf richtiges Pixel warten
*pixelzeiger=ADCH; // letzten ADC-Wert auslesen und wegwerfen
*pixelzeiger++=ADCH; // aktuellsten ADC-Werte speichern
}
sei();
for (i=0; i<16; gamma[i++]=0); // 16 gammawerte zählen
pixelzeiger=&pixel[0];
for (lines=0; lines < zeilen; lines++) gamma[*pixelzeiger++ >> 4]++;
g_sprung=5;
g_dunkel=0;
for (i=1; i<16; i++)
{
if (gamma[i] > gamma[i-1])
if ((gamma[i] - gamma[i-1]) > g_sprung)
if (!g_dunkel) g_dunkel=i;
}
g_hell=0;
for (i=14; i; i--)
{
if (gamma[i] > gamma[i+1])
if ((gamma[i] - gamma[i+1]) > g_sprung)
if (!g_hell) g_hell=i;
}
strich_links=0;
strich_rechts=0;
for (i=0; i<zeilen; i++)
{
if (((pixel[i] >> 4) == g_dunkel) && ((g_hell-g_dunkel)>1))
{
if (!strich_links) strich_links=i; // wenn linker Rand gefunden,
else strich_rechts=i; // weitersuchen bis rechter Rand
}
}
strich_mitte=(strich_links+strich_rechts)/2;
strich_breite=strich_rechts-strich_links;
return(strich_mitte);
}
int main(void)
{
uint8_t i;
uint8_t pow_l, pow_r, dir_l, dir_r;
uint8_t strich, keine_strich, out_l, out_r, abweich;
initRobotBase();
extIntOFF(); // schaltet den E_INT1-Port auf Eingang für den ADC
//powerON();
// ADC interne Referenz 2,56V, Ergebniss linksbündig, Kanal ADC4 (E_INT1)
ADMUX = (1<<REFS1) | (1<<REFS0) | (1<<ADLAR) | 4;
// setzte free running triggern
SFIOR = (0<<ADTS2) | (0<<ADTS1) | (0<<ADTS0);
// kein interupt, Wandler einschalten, prescaller /2
ADCSRA = (0<<ADIE) | (1<<ADEN) | (0<<ADPS2) | (0<<ADPS1) | (1<<ADPS0);
// Autotriggern bedeutet jetzt free running aktivieren, altes Flag löschen
ADCSRA |= (1<<ADATE) | (1<<ADIF);
// Initialisierung starten
ADCSRA |= (1<<ADSC);
// und noch die wohl eher unnötige Initiallesung
while (!(ADCSRA & (1<<ADIF)));
ADCSRA |= (1<<ADIF);
out_l=true;
out_r=false;
keine_strich=0;
dir_l=dir_r=BWD;
setMotorDir(dir_l,dir_r);
pow_l=pow_r=mpower;
setMotorPWM(pow_l/2,pow_r/2);
mSleep(mpower);
setMotorPWM(pow_l,pow_r);
do
{
strich=get_line(50,48);
abweich=2*abs(spur-strich);
if (strich)
{
out_l=out_r=false;
if (strich < (spur-15)) out_l=true;
if (strich > (spur+15)) out_r=true;
if (strich < spur)
{
pow_l=mpower+abweich;
pow_r=mpower-abweich;
}
else
{
pow_l=mpower-abweich;
pow_r=mpower+abweich;
}
}
else
{
if (out_l) {pow_l=mpower; pow_r=0;}
if (out_r) {pow_l=0; pow_r=mpower;}
}
setMotorPWM(pow_l,pow_r);
writeString_P("\n\r");
writeInteger(strich, DEC);
writeString_P("-");
writeInteger(abweich, DEC);
mSleep(100);
} while (1);
return(0);
}
und ein paar Videos der Tests. Zuerst die RN-Teststrecke:
Bild hier
(http://www.youtube.com/watch?v=txMYl7aKTBA)
Dann ein USB-Kabel, das am Boden liegt:
http://www.youtube.com/watch?v=NFQz49vDDfg
Selbes Kabel auf einem anderen Untergrund:
http://www.youtube.com/watch?v=paXpHySWn7g
http://www.youtube.com/watch?v=qVBPT5tGpjg
Ganz nett, wenn man anschaut, wie es mit der KI dieses Linienfolgers bestellt ist. Zudem scanne ich den Strich nur von einer Seite...
Ich kapier das nicht richtig.
kannst du den code mal so abspecken das er das bild in ein array schreibt?
Euch ist hoffentlich klar, dass dies alles hier keine "Ready to use"-Lösung ist. Die Kamera ist nur das sehr einfache Auge. Wie der Roboter das Gesehene interpetiert, ist eure Aufgabe:
Code:
#include "RP6RobotBaseLib.h"
uint8_t bildspeicher[1024], *bildzeiger; // 32*32=1KB * 8Bit Bildspeicher bereitstellen
void bild_einlesen(void)
{
uint8_t pixel[32],*pixelzeiger;
uint8_t i, zeilen, step, lines, rows, h_step, h_sync, h_delay;
zeilen=32; // Das fertige Bild soll 32 Zeilen haben
step=7; // sichtbares TV-Bild ist ca. 30-260=230/32 ergibt Zeilensprung=7
rows=0; // Anzahl der Spalten (32x32, rechengünstig,aber verzerrt)
do
{
lines=zeilen; // Anzahl der einzulesenden Zeilen
pixelzeiger=&pixel[0]; // Zeiger auf Start Pixelspeicher
cli();
// h_sync abwarten (syncsignal länger 40 bedeutet Seitenanfang)
do { h_sync=0; while (ADCH > 20); while (ADCH < 30) h_sync++; } while (h_sync < 40);
// 30-35 Zeilen Austastzeit überlesen (der Rest des hsyncs+nicht darstellbare BTX-Infos)
h_step=35; while (h_step) { while (ADCH > 20); while (ADCH < 30); h_step--; }
// Der Lesecursor befindet sich jetzt oben links im TV-Bild
// ab hier werden in step-Sprüngen in allen Zeilen jeweils das Pixel eingelesen,
// das sich im zeitlichen h_delay-Abstand vom linken TV-Bildrand befinden
// (= eine TV-Bildspalte)
while (lines--)
{
// auf die nächste gültige Zeile warten
h_step=step; while (h_step) { while (ADCH > 20); while (ADCH < 30); h_step--; }
// mit h_delay steuern wir nun den Pixel an
// Nach dem sync fängt das Bild etwas verzögert an (schwarzschulter), bei mir 20
// bei ca. 150 beginnt die 2.Schwarzschulter. Bei 150-20 möglichen Bildpunkten
// ergibt sich eine maximale Auflösung von 128 horizontal. Zusammen mit der
// vertikalen Auflösung von 230 kämen wir dann bei einem 8MHz-ATMega auf stolze
// 128*230 Bildpunkte.
h_delay=20+4*rows; while (h_delay--);
*pixelzeiger=ADCH; // letzten ADC-Wert auslesen und wegwerfen
*pixelzeiger++=ADCH; // aktuellsten ADC-Werte speichern
}
sei();
pixelzeiger=&pixel[0];
bildzeiger=&bildspeicher[32*rows];
for (i=0; i<32; i++) *bildzeiger++ = *pixelzeiger++;
}while (rows++ <zeilen);
}
int main(void)
{
uint16_t i, j;
initRobotBase();
extIntOFF(); // schaltet den E_INT1-Port auf Eingang für den ADC
//powerON();
// ADC interne Referenz 2,56V, Ergebniss linksbündig, Kanal ADC4 (E_INT1)
ADMUX = (1<<REFS1) | (1<<REFS0) | (1<<ADLAR) | 4;
// setzte free running triggern
SFIOR = (0<<ADTS2) | (0<<ADTS1) | (0<<ADTS0);
// kein interupt, Wandler einschalten, prescaller /2
ADCSRA = (0<<ADIE) | (1<<ADEN) | (0<<ADPS2) | (0<<ADPS1) | (1<<ADPS0);
// Autotriggern bedeutet jetzt free running aktivieren, altes Flag löschen
ADCSRA |= (1<<ADATE) | (1<<ADIF);
// Initialisierung starten
ADCSRA |= (1<<ADSC);
// und noch die wohl eher unnötige Initiallesung
while (!(ADCSRA & (1<<ADIF)));
ADCSRA |= (1<<ADIF);
while(1)
{
bild_einlesen();
for (i=0; i<32; i++)
{
for (j=0; j<32; j++)
{
if (bildspeicher[j+32*i] >90) writeString_P("*");
else writeString_P(" ");
}
writeInteger(i,DEC);
writeString_P("\n\r");
}
mSleep(200);
}
return(0);
}
Damit macht man z.B. 32x32-"Bilder", wenn das jemand braucht. Die Ausgabe dazu findet ihr im Anhang.
Ein kleines Filmchen zeigt, wie es funktioniert:
Bild hier
(http://www.youtube.com/watch?v=5PCvCAti1RY)
Hier wird übrigens nur per Wert > 90 ein Punkt gesetzt oder nicht. Quasi nur die Rohdaten ohne Bearbeitung.
Gruß
mic
Lesezeichen