Erweiterter Linienfolger (Abzweigungen und Sackgassen)

**Double_A** · 11.10.2010, 00:17

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...

**Double_A** · 19.10.2010, 15:31

Leute, testet den bitte mal...

**Sternthaler** · 19.10.2010, 18:24

Hallo Double_A,

testen kann ich aktuell nicht, da mein PC nach einem Plattenausfall noch nicht 'komplett' ist.

Von der Idee her ist das auch nicht schlecht. Ich bin mir nicht sicher, aber ich meine, dass schon mal jemand etwas Ähnliches, zumindest an der Kante entlang, hier gemacht hatte.

Mir sind an deinem Programm 3 Dinge aufgefallen.

1: Den 'Ausstieg' mit "return 0;" solltest du durch "while(1);" ersetzen.
Kontrollerprogramme haben kein Betriebssystem wohin es zurück gehen könnte.

2: Was wird (theoretisch) passieren, wenn der letzte Messwert immer um den Faktor 1.09 größer wird? Der Asuro sich also sehr lansam der Linie nähert?
Durch dein "if (lineData[LEFT] > 1.1 * lastLeft)" geht es also in den else-Zweig.
Dort wird nun der größere, letzte Messwert gespeichert.
Und wieder, und wieder, und wieder, ...
Du siehst worauf ich hinaus will?

3) Du 'reisst' fürchterlich an den Motoren mit der wahrscheinlich häufigen, extremen Richtungsänderung.
Aber hier kann ich wohl sagen, dass die Motoren / Getriebe das aushalten. Die Musik vom Asuro über die Motoren erzeugt, macht nichts anderes und noch viel häufiger

Gruß Sternthaler.

P.S.: Ich versuche mal meinen PC wieder weiter zu installieren.

**Double_A** · 19.10.2010, 19:07

Danke für deine Antwort...

Zu Punkt 2:
Das könnt ein Problem sein, aber das passiert praktisch ja nicht.
Bzw. eigentlich ist das auch der Sinn davon. Dass er immer den aktuellen Wert der Linie kennt, wenn z.B. der eine Teil der Bahn stärker beleuchtet wird oder so...

Zu 3.
Ich hatte zuerst mal nach jedem Durchgang eine kleine Pause gemacht 50ms oder so. Aber das war im Endeffekt schlimmer, da der Asuro (zwar weiter gefahren ist aber) sich dann immer zu stark gedreht, sodass er danach nurnoch stärker gegenlenken musste im nächsten Durchgang.
Dadurch hat er angefangen hin und zu "zittern".
Ohne Pause schmiegt er sich immer schön der Kurve an. ^^
Hast du dazu vllt einen besseren Weg?

**Sternthaler** · 19.10.2010, 19:28

Immer wieder gerne.

Bei Punkt 2) bin ich mir nicht so sicher, ob da nicht doch irgendwann einmal 'unendlich' rauskommt.
Die Variable lastLeft wird ja mit dem Sensormesswert sowieso nur bis 255 gehen können. Aber dann könnte der nächste Messwert (auch maximal 255) ja nie mehr 'größer als' 1.1*255 aus lastLeft werden. Dann ist die Reglung 'kaputt'.

Bessere Weg für Punkt 3)?
Oh je, da fällt mir nur der Asuro: Linienfolger mit PD-Regler von Waste ein. (Ist ein PID-Regler geworden.) Hier findest du den Code von Waste dazu.
ABER: Du fährst ja wirklich rückwärts. Waste fährt nur vorwärts.
Ich tippe mal, dass du da einen Vorteil bei einer verlorenen Linie (sozusagen eine Ecke) hast. Und auch, dass du an der Kante entlang fährst hat ja den Vorteil, dass dich Abzweigungen nicht stören sollten. Dann fährst du zwar immer in der gleichen Richtung ab, aber das ist ja besser als die Linie zu verlieren.
So, also habe ich da keine wirkliche bessere Lösung für dich.

Gruß Sternthaler

**funkheld** · 21.10.2010, 16:41

...Leute, testet den bitte mal...

hmmm..., kannst das nicht testen ob es astrein funktioniert?

**Double_A** · 25.10.2010, 20:29

Zitat von funkheld

...Leute, testet den bitte mal...

hmmm..., kannst das nicht testen ob es astrein funktioniert?

Also bei mir hatte er perfekt funktioniert, war nur ein bisschen lahm...

Aber irgenwie hast schon recht, in nem Asuro-Forum hat anscheinend niemand ein Asuro oder Lust irgendwas zu testen.

**radbruch** · 25.10.2010, 20:56

Hallo

Ich kann mir nicht vorstellen, dass solche Konstruktionen überhaupt funktionieren:

Code:

    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
...
            if (lineData[LEFT] > 1.1 * lastLeft) {
                // Asuro dreht sich nach links
                MotorDir(RWD, FWD);
                speedRight = SPEED * 1.5;
                speedLeft = SPEED * 1;

Eine bunte Mischung aus Integer- und Fließpunktzahlen. Ich könnte natürlich auch alles andere liegen lassen, meinen asuroclone reaktivieren und dein Programm testen. Und dann werde ich vielleicht feststellen, das diese Mischung eben nicht fiunktioniert. Also warte ich mal ab.

Gruß

mic

P.S.:
Ungeprüft würde ich das so formulieren:

Code:

            if (lineData[LEFT] > (lastLeft+lastLeft/10)) {
                // Asuro dreht sich nach links
                MotorDir(RWD, FWD);
                speedRight = SPEED + SPEED/2;
                speedLeft = SPEED;

**Double_A** · 25.10.2010, 23:01

Warum sollte das nicht gehen?
Die werden ja nur als Integer gespeichert (was auch iO ist), die Berechnung selbst kann ja schon Float sein...

Zumindest gings bei mit als ich es das letzte mal getestet hab.

**radbruch** · 25.10.2010, 23:16

die Berechnung selbst kann ja schon Float sein...

Das "kann" stört mich eben etwas. Wenn die Berechnung nur ganzzahlig ist, funktioniert es, weil es Lust dazu hat.

Da ich es ja im Moment nicht testen kann/will, was bedeutet eigentlich das:

Das einzige Problem ist die Geschwindigkeit...

Wie schnell ist dein Algorithmus im Vergleich zu diesem:

Bild hier

Code:

   FrontLED(ON);
   MotorSpeed(225,225);
   while(1)
   {
      LineData(data);
      if(data[0]<data[1]) MotorSpeed(200,250); else MotorSpeed(250,200);
/*
      PrintInt(data[0]);
      SerWrite("- ", 2);
      PrintInt(data[1]);
      SerWrite("\n\r", 2);
      Msleep(100);
*/
      Sleep(255);
   }

(Aus https://www.roboternetz.de/phpBB2/ze...=458371#458371)