Erweiterter Linienfolger (Abzweigungen und Sackgassen)
Hallo,
Da ich vor einigen Monaten für die Schule einen Linienfolger für den Asuro programmieren durfte, und ich nun nicht mehr in der "Szene" bin, möchte ich meine Software hier veröffentlichen. So kann sie verbessert und erweitert werden, statt auf meiner Festplatte zu vergammeln...
Zuerst noch eine ausführliche Beschreibung des Algorithmuses:
Code:
Asuro folgt nicht direkt der Linie sondern ihrer rechten Kante. Der linke Sensor befindet sich im Optimalfall immer auf der Linie. Der rechte Sensor befindet sich im Optimalfall immer rechts neben der Linie. Die Linie muss breiter sein als die Entfernung der Sensoren zu einander.
Die Sensoren überprüfen ihre Position unabhängig voneinander. Dazu vergleicht der Asuro den aktuellen Messwert mit dem zuletzt gemessenen Messwert. Stellt der Asuro einen grösseren Unterschied fest, wird der aktuelle Messwert nicht als Vergleichswert gespeichert, um zu vermeiden dass der Asuro die Linie nicht „vergisst“. Ausserdem wird die geeignete Kurskorrektur eingeleitet.
Kommt der linke Sensor vor der Linie ab, lenkt der Asuro immer nach links. Es ist in diesem Fall nie nötig nach rechts zu lenken, da die Linienbreite verhindert, dass der Sensor die Linie auf der linken Seite verlässt, da der rechte Sensor das schon davor ausgleicht.
Sollte der rechte Sensor sich über der Linie befinden, so lenkt der Asuro immer nach rechts.
Wenn sich beide Sensoren in der Optimalposition befinden, fährt der Asuro gerade aus.
Vor jedem Lichtsensoren-Messdurchgang werden die Tastsensoren überprüft. Es findet eine Doppelte Überprüfung statt, da die Tastsensoren auch ohne Berührung ausschlagen können. Wird eine Kollision erkannt stoppt der Asuro und beendet sein Programm.
Und hier noch den schön kommentierten SourceCode:
Code:
/*****************************************************************************
*
* File Name: main.c
* Project : Linienfolger
*
* Beschreibung: Folgt einer Linie mit Abzweigungen und Sackgassen.
* Der 1. Sensor befindet sich auf der Linie,
* der 2. Sensor rechts der Linie.
*
* Ver. Datum Author Kommentare
* ------- ---------- -------------- ------------------------------
* 1.0 02.06.2010 Amedeo A.
*
*****************************************************************************/
#include <asuro.h>
#define SPEED 130
int main(void) {
int lastLeft = 255, lastRight = 0; // Variablen für Vergleichswerte
int speedLeft, speedRight; // Variablen für Fahrgeschwindigkeiten
int lineData [2]; // Array für aktuelle Lichtsensoren-Messdaten
unsigned char t1, t2; // Variablen für aktuelle Tastsensoren-Messdaten
// Asuro wird für den Start vorbereitet (Initialisieren, FrontLED einschalten, Fahrrichtung: vorwärts)
Init();
FrontLED(ON);
MotorDir(FWD, FWD);
// Asuro fährt los
speedLeft = speedRight = SPEED;
MotorSpeed(speedLeft, speedRight);
// Eigentliches Linienfolger-Programm beginnt
while(1) {
// Doppellte Überprüfung der Tastsensoren (wegen Fehlalarme)
t1 = PollSwitch();
t2 = PollSwitch();
if (t1 && t2 && t1 == t2) {
// Wenn beide Messungen eine Kollision melden, wird das Programm beendet
MotorSpeed(0, 0);
return 0;
} else {
LineData(lineData); // Werte der Lichtsensoren auslesen
// Wenn linker Wert heller als die letzte Messung ist(Asuro ist von der Schwarzen Linie abgekommen)
if (lineData[LEFT] > 1.1 * lastLeft) {
// Asuro dreht sich nach links
MotorDir(RWD, FWD);
speedRight = SPEED * 1.5;
speedLeft = SPEED * 1;
} else {
// Aktuelle Messung wird als Vergleichswert für den nächsten Check gespeichert.
lastLeft = lineData[LEFT];
}
// Wenn rechter Wert dunkler als die letzte Messung ist("weisser" Sensor ist auf der Linie)
if (lineData[RIGHT] < 0.90 * lastRight) {
// Asuro dreht sich nach rechts
MotorDir(FWD, RWD);
speedLeft = SPEED * 1.5;
speedRight = SPEED * 1;
} else {
// Aktuelle Messung wird als Vergleichswert für den nächsten Check gespeichert.
lastRight = lineData[RIGHT];
}
// Wenn beiden Messungen innerhalb der Toleranz sind
if (lineData[LEFT] < 1.1 * lastLeft && lineData[RIGHT] > 0.90 * lastRight) {
// Asuro fährt gerade aus
speedLeft = speedRight = SPEED;
MotorDir(FWD, FWD);
}
// Ausgewähltes Fahrmanöver wird initialisiert
MotorSpeed(speedLeft, speedRight);
}
}
return 0;
}
Lizenz:
Bild hier
Diese Software steht unter einer Creative Commons Namensnennung-Nicht-kommerziell-Weitergabe unter gleichen Bedingungen 3.0 Unported Lizenz.
Anmerkungen:
Der Algorithmus funktioniert eigentlich sehr zuverlässig. Das einzige Problem ist die Geschwindigkeit... Etwas schneller wäre besser.
Falls jemand noch Verbesserungsmöglichkeiten sieht, bitte melden. ^^
PS: Zum Compilieren hatte ich damals die aktuellste AsuroLibrary benutzt...
