Code:
#include "RP6RobotBaseLib.h"
#define power 50
#define rampe 50
uint8_t bildspeicher[1024], *bildzeiger; // 32*32=1KB * 8Bit Bildspeicher bereitstellen
// 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;
}
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();
do { h_sync=0; while (ADCH > 20); while (ADCH < 30) h_sync++; } while (h_sync < 40);
h_step=35; while (h_step) { while (ADCH > 20); while (ADCH < 30); h_step--; }
while (lines--)
{
// auf die nächste gültige Zeile warten
h_step=step; while (h_step) { while (ADCH > 20); while (ADCH < 30); h_step--; }
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;
uint16_t gamma[16],g_hell, g_dunkel, g_hell_count;
uint8_t lichtpunkt, abstand;
uint8_t dir_l, dir_r, pow_l, pow_r;
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);
dir_l=dir_r=FWD;
setMotorDir(dir_l,dir_r);
pow_l=pow_r=power;
setMotorPWM(pow_l,pow_r); mSleep(rampe);
while(1)
{
bild_einlesen();
for (i=0; i<16; gamma[i++]=0); // 16 gammawerte zählen
bildzeiger=&bildspeicher[0];
for (i=0; i < 1024; i++) gamma[*bildzeiger++ >> 4]++;
g_hell=g_dunkel=0;
for (i=1; i<16; i++)
{
if (gamma[i] > g_hell) { g_dunkel=g_hell; g_hell=gamma[i]; }
else if (gamma[i] > g_dunkel) g_dunkel=gamma[i];
}
if (g_hell < g_dunkel)
{
i=g_hell;
g_hell=g_dunkel;
g_dunkel=i;
}
for (i=1; i<16; i++)
{
if (g_dunkel == gamma[i]) g_dunkel=i;
if (g_hell == gamma[i]) g_hell=i;
}
for (i=0; i<16; i++)
{
writeInteger(gamma[i], DEC);
if (gamma[i] == g_hell) writeString_P("H");
else if (gamma[i] == g_dunkel) writeString_P("D");
else writeString_P(" ");
}
writeString_P("\n\r");
writeInteger(g_dunkel, DEC);
writeString_P("-");
writeInteger(g_hell, DEC);
writeString_P("\n\r");
g_hell_count=0;
lichtpunkt=0;
for (i=0; i<32; i++)
{
for (j=0; j<32; j++)
{
//if (bildspeicher[j+32*i]>>4 == g_dunkel) writeString_P(" ");
//if (bildspeicher[j+32*i]>>4 > g_hell+1) writeString_P("*");
//else writeString_P(" ");
if (bildspeicher[j+32*i]>>4 > g_hell+1) g_hell_count++;
}
if ((!lichtpunkt) && (g_hell_count > 10)) lichtpunkt=i;
else g_hell_count=0;
writeInteger(i,DEC);
if (lichtpunkt) { writeString_P("-"); writeInteger(lichtpunkt, DEC);}
writeString_P("\n\r");
}
abstand=18;
if (lichtpunkt)
{
pow_l=pow_r=power/2;
setMotorPWM(pow_l,pow_r); mSleep(rampe);
if (lichtpunkt > abstand)
{
if (dir_l == BWD)
{
setMotorPWM(pow_l/2,pow_r/2); mSleep(rampe);
setMotorPWM(0,0); mSleep(rampe);
dir_l=dir_r=FWD;
setMotorDir(dir_l,dir_r);
setMotorPWM(pow_l/2,pow_r/2); mSleep(rampe);
setMotorPWM(pow_l,pow_r); mSleep(rampe);
}
}
if (lichtpunkt < abstand)
{
if (dir_l == FWD)
{
setMotorPWM(pow_l/2,pow_r/2); mSleep(rampe);
setMotorPWM(0,0); mSleep(rampe);
dir_l=dir_r=BWD;
setMotorDir(dir_l,dir_r);
setMotorPWM(pow_l/2,pow_r/2); mSleep(rampe);
setMotorPWM(pow_l,pow_r); mSleep(rampe);
}
}
if (lichtpunkt == abstand)
{
setMotorPWM(0,0); mSleep(rampe);
}
} // ende lichtpunkt != 0
else
{
if (dir_l == BWD) dir_l=dir_r=FWD;
setMotorDir(dir_l,dir_r);
if (pow_l < power)
{
pow_l=pow_r=power;
setMotorPWM(pow_l,pow_r); mSleep(rampe);
}
}
}
return(0);
}
Bei 32 Zeilen beträgt die messbare Distanz ca. 27 (Lichtpunkt hebt sich bei Tageslicht nicht mehr auf weisem Blatt ab oder Sehne des Kreissegment des Lichtpunkts kommt am unteren Bildrand nicht mehr auf 10)) und 3 (zu dicht an der Kamera). Das hängt natürlich sehr vom Aufbau ab, die Werte gelten bei Tageslicht und meinem Aufbau:
Lesezeichen