Ja, Liniensensoren habe ich gestern nochmal kalibriert. Hier der Code dazu. Das ist eine Kalibrierung mit anschließender Linienverfolgung.
/*
Ersteller: Frank Pelzhause
Datum: 18.11.2009
*/
/* Dieses Programm vereint eine Kombination aus Kalibrierung der Line Sensoren
und ein Pr?fprogramm f?r die Fahrt ?ber eine Linie.
Einige Hilfsfunktionen sind aus bestehenden Programmen ?bernommen,
aber der Ablauf ist versuchsweise nur in dieser Datei hinterlegt.
F?r weitere Verwendung k?nnen die Funktionen auch in einzelne Dateien
und Headerdateien ausgelagert werden.
Das Ganze ist nat?rlich ausbauf?hig.
Eine Kurzbeschreibung der Funktionen ist immer am Anfang dokumentiert,
weitere Informationen sind jeweils vor der Funktion beschrieben.
Spezielle Anmerkungen sind inline Dokumentiert. */
/* Ben?tigte Include Dateien */
#include <nibobee/iodefs.h> /* muss immer als erste Datei
eingebunden werden */
#include <nibobee/led.h> /* LED Funktionen */
#include <nibobee/delay.h> /* Verz?gerungen, Includiert
die avr/delay.h
(achtung: Zusatzfunktionen) */
#include <nibobee/sens.h> /* Aktivierung der F?hler */
#include <nibobee/analog.h> /* ????? */
#include <nibobee/line.h> /* Liniensensoren (Infrarot vorne) */
#include <nibobee/motpwm.h> /* Motorsteuerung */
/* Spezielle Includes, keine Ahnung warum (Kommentare erw?nscht) */
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
/* Definieren der verschiedenen Modes,
um die Folgereaktionen auswerten zu k?nnen */
enum {
MODE_KALIBRIEREN,
MODE_DRIVE,
MODE_LAUFLICHT,
};
/* Funktion um ein Lauflicht zu starten.
Interuptsteuerung wird aktiviert, um auch w?hrend der Funktion
reagieren zu k?nnen und ein Ereignis zur?ckzuliefern */
uint8_t Lauflicht()
{
led_init();
sens_init();
while(1==1)
{
enable_interrupts();
int ledNr;
for (ledNr=0; ledNr<4; ledNr++)
{
led_set(ledNr, 1);
delay(350);
led_set(ledNr, 0);
delay(150);
}
/* Test auf F?hlerbet?tigung */
/* Wenn beide F?hler nach hinten gedr?ckt wurden
wird ein Status MODE_KALIBRIEREN zur?ckgegeben
Die F?hler m?ssen allerdings f?r eine ganze
Umdrehung der LEDs festgehalten werden,
da die Abfrage erst nach der For-Schleife erfolgt */
if ((sens_getLeft()==-1) && (sens_getRight()==-1))
{
while ((sens_getLeft()==-1) && (sens_getRight()==-1)) {
delay(1);
}
led_init();
return MODE_KALIBRIEREN;
}
}
return 0;
}
/* Calibrierung */
/*
Hier werden die Linesensoren kalibriert.
Die Werte werden in das EEPROM geschrieben, wenn der Befehl dazu gegeben wurde.
- Ablauf der Kalibrierung:
1. Linker F?hler nach hinten dr?cken: Wei?abgleich, Dazu Nibobee auf ein wei?es
Blatt Papier stellen. Dioden blinken.
2. Rechter F?hler nach hinten dr?cken: Schwarzabgleich, Dazu Nibobee auf ein schwarzes
Blatt Papier stellen. Dioden blinken.
3. Zum speichern der Daten beide F?hler nach Vorne dr?cken.
Die Daten werden dann dauerhaft im EEPROM gespeichert.
Dioden Blinken abermals und das linienverfolgungsprogramm wird gestartet.
*/
/* Deklarationen */
void line_read_persistent();
uint16_t line_get(uint8_t idx);
void line_calibrate_white();
void line_calibrate_black();
/* Blinkfunktion */
void do_blink(uint8_t mask) {
for (int i=0; i<5; ++i) {
led_set(LED_L_RD, mask&0x01);
led_set(LED_R_RD, mask&0x01);
led_set(LED_L_YE, mask&0x02);
led_set(LED_R_YE, mask&0x02);
delay(200);
led_set(LED_L_RD, 0);
led_set(LED_R_RD, 0);
led_set(LED_L_YE, 0);
led_set(LED_R_YE, 0);
delay(200);
}
}
/* Kalibrieren im eigentlichen Sinn.
Besonderheit: Erst nach bet?tigung Beider F?hler nach Vorne
wird nach dem speichern in den Modus "Linienvervolgung umgeschaltet */
uint8_t Kalibrieren() {
led_init();
motpwm_init();
sens_init();
analog_init();
activate_output_bit(IO_LINE_EN);
line_readPersistent();
while(1==1) { // Endlosschleife (1==1 ist immer wahr!)
enable_interrupts();
delay(10);
if (sens_getLeft()==-1) {
while (sens_getLeft()==-1) delay(1); /* Linker F?hler nach hinten --> Wei?abgleich */
line_calibrateWhite();
do_blink(2);
}
if (sens_getRight()==-1) {
while (sens_getRight()==-1) delay(1); /* Linker F?hler nach hinten --> Wei?abgleich */
line_calibrateBlack();
do_blink(1);
}
set_output_groupbitval(IO_LEDS, L_YE, line_get(LINE_L)>160);
set_output_groupbitval(IO_LEDS, L_RD, line_get(LINE_L)>240);
set_output_groupbitval(IO_LEDS, R_YE, line_get(LINE_R)>160);
set_output_groupbitval(IO_LEDS, R_RD, line_get(LINE_R)>240);
/* Beide F?hler nach vorne = Speichern der Werte und weiter
zum Linienprogramm */
if ((sens_getLeft()==1) && (sens_getRight()==1)) {
while ((sens_getLeft()==1) && (sens_getRight()==1)) {
delay(1);
}
line_writePersistent();
do_blink(3);
return MODE_DRIVE;
}
}
return 0;
}
/* Linien fahren */
/* Das Programm l?uft solange, bis beide F?hler nach hinten get?tigt werden,
dann wird die Kalibrierung wieder mit dem Lauflicht gestartet. */
uint8_t run_line()
{
activate_output_group(IO_LEDS); // LED bits als Output
sens_init();
motpwm_init();
motpwm_setLeft(0);
motpwm_setRight(0);
analog_init();
line_readPersistent();
set_output_group(IO_SENS); // Pull-ups aktivieren
activate_output_bit(IO_LINE_EN);
int16_t speed_flt_l=0;
int16_t speed_flt_r=0;
// Countdown: LEDs blinken lassen
for (uint8_t i=0; i<5; ++i) {
led_set(LED_L_RD, 1);
led_set(LED_R_RD, 1);
_delay_ms(10);
led_set(LED_L_RD, 0);
led_set(LED_R_RD, 0);
_delay_ms(990);
}
led_set(LED_L_YE, 1);
led_set(LED_R_YE, 1);
_delay_ms(1000);
led_set(LED_L_YE, 0);
led_set(LED_R_YE, 0);
// Hauptschleife:
while(1) {
enable_interrupts(); /* F?hler aktivieren */
sei();
_delay_ms(1);
int16_t speed_l=0;
int16_t speed_r=0;
int16_t lval = line_get(LINE_L);
int16_t cval = line_get(LINE_C);
int16_t rval = line_get(LINE_R);
if (lval+cval+rval < 20) {
led_set(LED_L_RD, 0);
led_set(LED_R_RD, 0);
speed_r=300, speed_l=300;
} else if ((lval<cval) && (lval<rval)) {
// lval is minimum
led_set(LED_L_RD, 1);
led_set(LED_R_RD, 0);
speed_r=350, speed_l=250-1*(cval-lval);
} else if ((rval<cval) && (rval<lval)) {
// rval is minimum
led_set(LED_L_RD, 0);
led_set(LED_R_RD, 1);
speed_r=250-1*(cval-rval), speed_l=450;
} else {
// cval is minimum
led_set(LED_L_RD, 1);
led_set(LED_R_RD, 1);
speed_r=450 + 1*(rval-cval), speed_l=450 + 1*(lval-cval);
}
speed_flt_l*=3; speed_flt_l+=speed_l; speed_flt_l/=4;
speed_flt_r*=3; speed_flt_r+=speed_r; speed_flt_r/=4;
motpwm_setLeft(speed_flt_l);
motpwm_setRight(speed_flt_r);
/* Wenn beide F?hler nach hinten gedr?ckt werden,
wird wieder das Lauflicht aktiviert und eine erneute
Kalibrierung kann erfolgen */
if ((sens_getLeft()==-1) && (sens_getRight()==-1)) {
while ((sens_getLeft()==-1) && (sens_getRight()==-1)) {
delay(1);
}
int16_t speed_l=0;
int16_t speed_r=0;
motpwm_setLeft(speed_l);
motpwm_setRight(speed_r);
return MODE_LAUFLICHT;
}
}
return 0;
}
/* Hauptprogramm (main())
Schlicht und einfach :-)) */
uint8_t mode = MODE_LAUFLICHT;
int main() {
while(1==1) {
enable_interrupts();
delay(1);
switch (mode) {
case MODE_LAUFLICHT: mode = Lauflicht(); break;
case MODE_KALIBRIEREN: mode = Kalibrieren(); break;
case MODE_DRIVE: mode = run_line(); break;
}
}
return 0;
}
Mit der mitgelieferten Kalibration funktioniert es auch nicht besser.
Irgendwie hab ich das Gefühl, dass einer der Phototransistoren defekt ist. Kann man die irgendiwe einzeln testen?
Viele Grüße,
Manuel