## c't-Projekte - Mailinglisten

[Voriger (Datum)] [Nächster (Datum)] [Voriger (Thread)] [Nächster (Thread)]
[Nach Datum][Nach Thread]

### Re: [ct-bot] Motorregelung / bot_follow_line

Absender: Timo Sandmann
Datum: Fr, 21.09.2007 18:17:36
In-reply-to: <EDD9B1E4-330E-49D9-B448-5B9FDE19408A@xxxxxxxxxxxxxxx>
References: <000101c7efeb$c1eb3a00$fe78a8c0@mexpnew> <EDD9B1E4-330E-49D9-B448-5B9FDE19408A@xxxxxxxxxxxxxxx>

    Am 21.09.2007 um 18:01 schrieb Timo Sandmann:

>     Hallo,

> `anbei ein kleiner Patch mit geändertem Linienfolger. Der` `funktioniert bei mir mit Motorregelung prima und kommt auch mit` `spitzen Winkeln klar.` `Der Bot fährt damit auf der rechten Kante der Linie, die mindestens` `so breit wie die beiden CNY70 sein sollte (ich hab's mit schwarzem` `Isolierband probiert).`

> `Vielleicht kann das mal jemand mit seinem Bot testen und berichten,` `ob es nun besser klappt.`

> `Zwei kleine (vorläufige )Demo-Videos gibt es unter <http://` `www.timosandmann.de/bot/line.html>.`

>     Viele Grüße,
>     Timo

    Wo auch immer der txt-Anhang geblieben ist... hier noch mal:

`Index: /Volumes/user/Users/ts/Documents/workspace/ct-Bot/bot-logic/` `behaviour_follow_line.c`

    ===================================================================

`--- /Volumes/user/Users/ts/Documents/workspace/ct-Bot/bot-logic/` `behaviour_follow_line.c (revision 1253)` `+++ /Volumes/user/Users/ts/Documents/workspace/ct-Bot/bot-logic/` `behaviour_follow_line.c (working copy)`

    @@ -20,8 +20,8 @@
    /*!
      * @file 	behaviour_follow_line.c
      * @brief 	Linienverfolger
    - * @author 	Torsten Evers (tevers@xxxxxxxxxxxxx)
    - * @date 	03.11.06
    + * @author 	Timo Sandmann (mail@xxxxxxxxxxxxxxx)
    + * @date 	21.09.2007
      */
    #include "bot-logic/bot-logik.h"
    @@ -27,152 +27,70 @@
    #include "bot-logic/bot-logik.h"
    #ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE
    -/* Konstanten fuer das Verhalten */
    -#define CORNER_LEFT					1
    -#define CORNER_RIGHT				2
    -/* Zustaende fuer das Verhalten */

`-#define CHECK_LINE 0 /* !< Feststellen ob wir ueber einer Linie` `sind */` `-#define FOLLOW_LINE 1 /* !< Folgen einer geraden oder leicht` `gekruemmten Linie */` `-#define CHECK_BORDER 2 /* !< Abgrundsensoren haben Alarm` `geschlagen. Feststellen ob wirklich Abgrund oder Ecke */` `-#define CORNER_TURN 3 /* !< Drehung in Richtun detektiertem` `Abgrund */` `-#define CORRECT_POS 4 /* !< Nach der Drehung 1cm vorfahren zum` `Ausgleichen */` `-#define ADVANCE_CORNER 5 /* !< Auf die Ecke zufahren, bis die` `Linie verschwindet */` `-#define RETREAT_AND_STOP 6 /* !< Zurueckfahren mit voller` `Geschwindigkeit, dann Stop und Verhalten verlassen */`

    +#include "timer.h"
    -/* Status- und Hilfsvariablen */
    -static int8 lineState=CHECK_LINE;
    -static int8 cornerDetected=False;
    +/*!

`+ * Zeit zwischen zwei Korrekturen [ms]. Groessere Werte bewirken` `"ruhigeres" Fahren,` `+ * erhoehen damit aber auch die Reaktionszeit (z.B. bei scharfen` `Kurven problematisch)`

    + */
    +#define	CORRECTION_DELAY	150
    /*!
    - * Folgt einer Linie, sobald beide Liniensensoren ausloesen
    - * Die Linie sollte in etwa die Breite beider CNY70 haben

`+ * Folgt einer Linie. Der linke Liniensensor ist dabei auf der` `Linie, der Rechte daneben.` `+ * Der Bot faehrt also auf der rechten Kante der Linie. Sie sollte` `in etwa die Breite`

    + * beider CNY70 haben.
      * @param *data	Verhaltensdatensatz
      */
    -void bot_follow_line_behaviour(Behaviour_t *data) {
    -	switch (lineState) {
    -	case CHECK_LINE: /* sind beide Sensoren ueber einer Linie? */
    -		if (sensLineL>=LINE_SENSE&& sensLineR>=LINE_SENSE) {

`- /* zunaechst alle Hilfsverhalten ausschalten, die den Algorithmus` `stoeren */`

    -			/* Abgrund- und Kollisions-Verhalten ausschalten */
    -#ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
    -			deactivateBehaviour(bot_avoid_col_behaviour);
    -#endif
    -#ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE
    -			deactivateBehaviour(bot_avoid_border_behaviour);
    -#endif
    -			/* bot_glance() stoert bot_turn() */
    -			//deactivateBehaviour(bot_glance_behaviour);
    -			/* losfahren und nach FOLLOW_LINE wechseln */
    -			speedWishLeft=BOT_SPEED_FOLLOW;
    -			speedWishRight=BOT_SPEED_FOLLOW;
    -			lineState=FOLLOW_LINE;
    -		}
    -		break;
    +void bot_follow_line_behaviour(Behaviour_t * data) {
    +	static int16_t lastLeft = 0;
    +	static int16_t lastRight= 0;
    +	static uint8_t lastCorrection = 0;
    +	static uint32_t lastCorrectionTime = 0;
    +	uint8_t correction = 0;
    +
    +	if (sensLineL >= LINE_SENSE && sensLineR < LINE_SENSE) {
    +		/* Bot faehrt auf rechter Linienkante */
    +		speedWishLeft  = BOT_SPEED_SLOW;
    +		speedWishRight = BOT_SPEED_SLOW;
    +	} else if (sensLineL < LINE_SENSE) {
    +		/* Bot fahert rechts neben der Linie */
    +		speedWishLeft  = -BOT_SPEED_FOLLOW;
    +		speedWishRight = BOT_SPEED_FOLLOW;
    +		correction = 1;
    +	} else {
    +		/* Bot faehrt auf der Linie */
    +		speedWishLeft  = BOT_SPEED_FOLLOW;
    +		speedWishRight = -BOT_SPEED_FOLLOW;
    +		correction = 2;
    +	}
    -	case FOLLOW_LINE:
    -		/* Pruefen, ob die Abgrundsensoren einen Abgrund sehen */
    -		if (sensBorderL>BORDER_DANGEROUS|| sensBorderR>BORDER_DANGEROUS) {

`- /* Abgrund erkannt, das kann jetzt eine Linie sein oder ein` `richtiger Abgrund.*/`

    -			if (sensBorderL>BORDER_DANGEROUS&& sensBorderR>BORDER_DANGEROUS) {
    -				/* Wenn beidseitig erkannt, koennen wir damit nicht umgehen ->
    -				 * Ende des Verhaltens */
    -				speedWishLeft=BOT_SPEED_STOP;
    -				speedWishRight=BOT_SPEED_STOP;
    -				//LOG_INFO("Stopp in FOLLOW_LINE");
    -				return_from_behaviour(data);
    -				break;
    -			}

`- /* nachsehen, ob der linke oder rechte Liniensensor ohne Kontakt` `zur Linie ist`

    -			 * und ggfs. gegensteuern */
    -			if (sensBorderL>BORDER_DANGEROUS) {
    -				cornerDetected=CORNER_LEFT;
    -			} else {
    -				cornerDetected=CORNER_RIGHT;
    -			}
    -			/* nun zur vermuteten Ecke vorfahren */
    -			lineState=CHECK_BORDER;
    -			bot_drive_distance(data, 0, BOT_SPEED_FOLLOW, 3);
    -			break;
    -		}
    -		if (sensLineL<LINE_SENSE&& sensLineR>LINE_SENSE) {
    -			/* links von der Linie abgekommen, daher nach rechts drehen */
    -			//LOG_DEBUG("Drehe rechts");
    -			speedWishLeft=BOT_SPEED_FOLLOW;
    -			speedWishRight=-BOT_SPEED_FOLLOW;
    -		} else if (sensLineL>LINE_SENSE&& sensLineR<LINE_SENSE) {
    -			/* andersrum, also links drehen */
    -			//LOG_DEBUG("Drehe links");
    -			speedWishLeft=-BOT_SPEED_FOLLOW;
    -			speedWishRight=BOT_SPEED_FOLLOW;
    -		} else if (sensLineL>LINE_SENSE&& sensLineR>LINE_SENSE) {
    -			/* noch ueber der Linie, also einfach geradeaus weiter */
    -			//LOG_DEBUG("Fahre geradeaus");
    -			speedWishLeft=BOT_SPEED_FOLLOW;
    -			speedWishRight=BOT_SPEED_FOLLOW;
    -		}
    -		break;
    -
    -	case CHECK_BORDER:
    -		/* wir sollten jetzt direkt an der Kante zum Abgrund stehen, wenn es
    -		 * denn wirklich eine ist. In dem Fall fahren wir ein Stueck zurueck.
    -		 * sonst gehen wir von einer Linie aus, drehen uns in die Richtung,
    -		 * in der wir den "Abgrund" entdeckt haben und machen dann weiter mit
    -		 * der Linienverfolgung */
    -		if (sensBorderL>BORDER_DANGEROUS|| sensBorderR>BORDER_DANGEROUS) {
    -			/* scheint wirklich ein Abgrund zu sein */
    -			lineState=RETREAT_AND_STOP;
    -			speedWishLeft=-BOT_SPEED_MAX;
    -			speedWishRight=-BOT_SPEED_MAX;
    -			break;
    -		}

`- /* war nur eine Ecke, noch weiter darauf zu bis kein Kontakt mehr` `zur Linie */`

    -		lineState=ADVANCE_CORNER;
    -		speedWishLeft=BOT_SPEED_FOLLOW;
    -		speedWishRight=BOT_SPEED_FOLLOW;
    -		break;
    -
    -	case ADVANCE_CORNER:
    -		/* auf die Ecke zufahren, bis die Linie verschwindet */
    -		if (sensLineL<LINE_SENSE&& sensLineR<LINE_SENSE) {
    -			/* Linie weg, also Stop, kurz zurueck und drehen */
    -			lineState=CORNER_TURN;
    -			speedWishLeft=-BOT_SPEED_SLOW;
    -			speedWishRight=-BOT_SPEED_SLOW;
    -			break;
    -		}
    -		speedWishLeft=BOT_SPEED_FOLLOW;
    -		speedWishRight=BOT_SPEED_FOLLOW;
    -		break;
    -
    -	case CORNER_TURN:
    -		/* 90 Grad in Richtung des detektierten Abgrunds drehen */
    -		lineState=CORRECT_POS;
    -		bot_turn(data, (cornerDetected==CORNER_LEFT) ? 90 : -90);
    -		cornerDetected=False;
    -		break;
    -
    -	case CORRECT_POS:
    -		lineState=FOLLOW_LINE;
    -		bot_drive_distance(data, 0, BOT_SPEED_SLOW, 2);
    -		break;

`+ if (lastCorrection != correction && !timer_ms_passed` `(&lastCorrectionTime, CORRECTION_DELAY)) {` `+ /* Falls die letzte Korrektur gerade erst war, reagieren wir` `(noch) nicht */`

    +		speedWishLeft  = lastLeft;
    +		speedWishRight = lastRight;
    +		return;
    +	}
    -	case RETREAT_AND_STOP:
    -		/* wir sind an einem Abgrund, Stop und Ende des Verhaltens */
    -		speedWishLeft=BOT_SPEED_STOP;
    -		speedWishRight=BOT_SPEED_STOP;
    -		return_from_behaviour(data);
    -		break;
    -	}
    +	/* neue Werte merken */
    +	lastCorrection = correction;
    +	lastLeft  = speedWishLeft;
    +	lastRight = speedWishRight;
    }
    /*!
    - * Folgt einer Linie, sobald beide Liniensensoren ausloesen
    - * Die Linie sollte in etwa die Breite beider CNY70 haben

`+ * Folgt einer Linie. Der linke Liniensensor ist dabei auf der` `Linie, der Rechte daneben.` `+ * Der Bot faehrt also auf der rechten Kante der Linie. Sie sollte` `in etwa die Breite`

    + * beider CNY70 haben.
      * @param	*caller Verhaltensdatensatz des Aufrufers
      */
    -void bot_follow_line(Behaviour_t *caller) {
    +void bot_follow_line(Behaviour_t * caller) {
    	switch_to_behaviour(caller, bot_follow_line_behaviour, NOOVERRIDE);
    -	lineState=CHECK_LINE;
    -	cornerDetected=False;
    +	/* stoerende Notfallverhalten aus */
    +#ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
    +	deactivateBehaviour(bot_avoid_col_behaviour);
    +#endif
    +#ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE
    +	deactivateBehaviour(bot_avoid_border_behaviour);
    +#endif
    }
    #endif	// BEHAVIOUR_FOLLOW_LINE_AVAILABLE

- Antworten:
  - AW: [ct-bot] Motorregelung / bot_follow_line
    - From: Frank Menzel

- Referenzen:
  - [ct-bot] Motorregelung
    - From: Frank Menzel
  - Re: [ct-bot] Motorregelung / bot_follow_line
    - From: Timo Sandmann

- Voriger (Datum): Re: [ct-bot] Motorregelung / bot_follow_line, Timo Sandmann
- Nächster (Datum): [ct-bot] Wiedereinstieg, Mopsos
- Voriger (Thread): Re: [ct-bot] Motorregelung / bot_follow_line, Timo Sandmann
- Nächster (Thread): AW: [ct-bot] Motorregelung / bot_follow_line, Frank Menzel
- Indizes:
  - Datum
  - Thread