- heise online
- c't Magazin
- iX Magazin
- Technology Review
- Telepolis

- heise mobil
- heise Security
- heise Netze
- Open Source
- heise Developer

- heise Resale
- heise Foto
- heise Autos
- c't-TV

- Software/Download
- Stellenangebote
- Preisvergleich
- IT-Markt

- heisetreff
- heise Veranstaltungen
- heise Whitepapers
- Abo&Heft
- Archiv

####

####

c't Projekte - c't-Bot und c't-Sim - Mailinglisten

## c't-Bot und c't-Sim

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

### [ct-bot] erweiterter Linienfolger-neues Verhalten

Absender: Frank Menzel
Datum: Mi, 25.02.2009 11:59:15

    Index: C:/eclipse/workspace/ct-Bot/bot-logic/bot-logik.c
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/bot-logic/bot-logik.c	(revision 1552)
    +++ C:/eclipse/workspace/ct-Bot/bot-logic/bot-logik.c	(working copy)
    @@ -288,6 +288,11 @@
     		insert_behaviour_to_list(&behaviour, new_behaviour(71, bot_calc_wave_behaviour, INACTIVE));
         #endif

    +	#ifdef BEHAVIOUR_FOLLOW_LINE_ENHANCED_AVAILABLE
    +	  	// erweiterter Linienfolge, der mit Unterbrechungen und Hindernissen klarkommt
    +		insert_behaviour_to_list(&behaviour, new_behaviour(71, bot_follow_line_enhanced_behaviour, INACTIVE));
    +	#endif
    +
     	#ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE
     		// Verhalten um einer Linie zu folgen
     		insert_behaviour_to_list(&behaviour, new_behaviour(70, bot_follow_line_behaviour, INACTIVE));
    Index: C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_follow_line.c
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_follow_line.c	(revision 1552)
    +++ C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_follow_line.c	(working copy)
    @@ -31,6 +31,7 @@
     #include "bot-logic/bot-logik.h"

     #ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE
    +#include "math_utils.h"

     //#define DEBUG_BEHAVIOUR_FOLLOW_LINE // Schalter fuer Debug-Code

    @@ -421,4 +422,184 @@
     }

     #endif	// VERSION
    +
    +
    +#ifdef BEHAVIOUR_FOLLOW_LINE_ENHANCED_AVAILABLE
    +
    +// Merkposition nach Umrunden Hindernis; erst wieder gewisse Strecke fahren bis Linienfolger abgebrochen werden kann fuer Hindernis
    +static position_t oldpos;
    +
    +// Statusvariable des Verhaltens
    +static uint8_t line_state_enh=0;
    +
    +// naechster Verhaltenseinsprung nach cancel
    +static uint8_t next_linestate=0;
    +
    +// Var fuer Positionsberechnung nach Unterbrechung
    +position_t point;
    +
    +// Verhaltenszustaende
    +#define START_ENH           0
    +#define START_LINE_ENH      1
    +#define CHECK_BREAK_IN_LINE 2
    +#define START_SOLVE_MAZE    3
    +#define TURN_AROUND_HAZARD  4
    +#define GO_AROUND_HAZARD    5
    +#define OTHER_SIDE_ON_LINE  6
    +#define TURN_NEXT_PART      7
    +#define SEARCH_LINE         8
    +#define END_LINE_ENH        99
    +
    +/* Check-Routine zum Erkennen ob sich bot schon auf der Linie befindet
    + * @return True wenn Linie erkannt wurde
    + */
    +static uint8_t check_line_sensors(void) {
    +	if (sensLineL >= LINE_SENSE || sensLineR >= LINE_SENSE) {
    +		line_state_enh = next_linestate; // jeweils naechster Eintrittspunkt
    +		return True;
    +	}
    +	return False;
    +}
    +
    +/* Check-Routine zum Erkennen ob sich linker Liniensensor allein schon auf der Linie befindet; dies waere
    + * ja optimal fuer den Linienfolger, da dieser ja auf linker Kante faehrt; wird verwendet nach Umrunden des
    + * Hindernisses, um sich wieder auf Linie in richtiger Richtung, also vom Hindernis weg, auszurichten
    + * @return True wenn linker Liniensensor allein auf Linie steht
    + */
    +static uint8_t check_left_line_sensor(void) {
    +	if (sensLineL >= LINE_SENSE && sensLineR <LINE_SENSE)
    +		return True;
    +	return False;
    +}
    +
    +/* Check-Routine zum Erkennen, ob sich bot an einer Linienunterberechung befindet oder Hindernis gesehen wird; wird
    + * verwendet zum Abbrechen des Linienfolgers an Unterbrechung oder Hindernis
    + * @return True wenn Unterbrechung erkannt wird oder Hindernis voraus gesehen wird
    + */
    +static uint8_t check_sensors(void) {
    +  // Unterbrechung wird daran erkannt, wenn sich der Linienfolger wegen keiner Linie mehr voraus nach links dreht und dabei
    +  // der rechte Abgrundsensor die unterbrochene Linie erkennt; Unterbrechung darf auch nur ca. maximal 3cm sein
    +  if (sensBorderR >= BORDER_DANGEROUS && sensLineL<LINE_SENSE && sensLineR<LINE_SENSE) {
    +		line_state_enh=CHECK_BREAK_IN_LINE;
    +		return True;
    +	}
    +
    +	// Abstand seit letztem Check ermitteln; nur bei Ueberschreitung des Mindestabstandes seit letzter Hindernisumrundung
    +	// darf wegen Hindernis abgebrochen werden, da sonst nach Umrundung und gleichem Hindernis beim Ausrichten des Linienfolgers
    +	// dieser sonst gleich wieder moeglicherweise abgebrochen wird
    +	uint16_t diff = get_dist(x_pos, y_pos, oldpos.x, oldpos.y);
    +	if ((diff >= 100 * 100) || (oldpos.x==0 && oldpos.y==0)) {
    +	  // Merkpos auf 0, wenn Abstand ueberschritten wurde
    +	  oldpos.x=0;
    +	  oldpos.y=0;
    +	  if (sensDistL <= OPTIMAL_DISTANCE || sensDistR <=OPTIMAL_DISTANCE) {
    +		line_state_enh=TURN_AROUND_HAZARD;
    +		return True;
    +	  }
    +	}
    +
    +	return False;
    +}
    +
    +
    +
    +/*! erweiterter Linienfolger, der auch Linienunterbrechungen und Hindernisse handhabt, waehrend der Bot die Linie verfolgt;
    + *  die Linienunterbrechung darf nur relativ klein sein (~3cm), so dass sich beim drehen am Ende der Linie der rechte Abgrundsensor
    + *  ueber den Neubeginn der unterbrochenen Linie drehn muss
    +  * @param *data	Verhaltensdatensatz
    + */
    +void bot_follow_line_enhanced_behaviour(Behaviour_t * data) {
    +	switch (line_state_enh) {
    +	case START_ENH: // maximal 60cm vorwaerts bis Linie voraus
    +	  bot_goto_dist(data, 600, 1);
    +	  next_linestate=START_LINE_ENH;  // Eintritt nachdem Cancel Linie gefunden hat
    +	  bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_line_sensors); // Ende der Vorwaertsfahrt wenn Linie erkannt wurde
    +	  line_state_enh=END_LINE_ENH;  // Ende falls keine Linie voraus gefunden wird
    +	break;
    +
    +	case START_LINE_ENH:              // Einsprung falls sich bot nun auf Linie befindet, weiter mit Linienfolger
    +	    bot_follow_line(data);        // Linienfolger laeuft los
    +		bot_cancel_behaviour(data, bot_follow_line_behaviour, check_sensors); // Abbruch Linienfolger bei Unterbrechung oder Hindernis
    +		line_state_enh=END_LINE_ENH;  // einfach mal auf Ende, bei Cancel Linienfolger wird dort Einsprung gesetzt
    +		break;
    +
    +	case CHECK_BREAK_IN_LINE: // Einsprung falls Linienunterbrechung erkannt wurde
    +        // Linie geht ja weiter nach Unterbrechung genau dort, wo rechter Abgrundsensor sich befindet; dort also Punkt berechnen und anfahren
    +		point = calc_point_in_distance(heading, BORDERSENSOR_POS_FW - 10, -BORDERSENSOR_POS_SW);
    +		bot_goto_pos(data, point.x, point.y, 999);
    +	    line_state_enh=START_LINE_ENH;  // Linienfolger sollte hier wieder greifen nach Ueberwindung der Unterbrechung
    +	break;
    +
    +   case TURN_AROUND_HAZARD:  // auf jeden Fall erst mal in Ausgangsrichtung drehen
    +        //ToDo: beim Drehen in beide Richtungen ermitteln, ob schon Kante, also Ende, des Hindernisses (Stein auf Linie) gesehen wird und
    +        //weil umgehen in diese Richtung kuerzer ist, dortlang drehen und Hindernisumgeher starten; solve_maze geht leider nur wenn bot sich
    +        // hier links dreht und dann rechts die Wand hat
    +        bot_turn(data, 70);  //Links drehen
    +	    line_state_enh=GO_AROUND_HAZARD;  // naechsten Einsprung setzen
    +		break;
    +
    +  case GO_AROUND_HAZARD:  // etwas vorwaerts fahren um von Linie wegzukommen
    +        //ToDo_beim Drehen in beide Richtungen ermitteln, ob schon Kante, also Ende, des Hindernisses (Stein auf Linie) gesehen wird und
    +        //weil umgehen in diese Richtung kuerzer ist, dortlang drehen und Hindernisumgeher starten
    +        bot_goto_dist(data,50,1);  // etwas vorwaerts fahren um von Linie wegzukommen
    +	    line_state_enh=START_SOLVE_MAZE;  // naechster Einsprung: Start solve_maze zum Umrunden des Hindernisses
    +		break;
    +
    +	case START_SOLVE_MAZE:    // Start Verhalten solve_maze zum Umrunden des Hindernisses
    +        bot_solve_maze(data); // Start solve_maze
    +        next_linestate=OTHER_SIDE_ON_LINE;  // Eintritt nachdem Cancel Linie erkannt hat
    +        // Solve_maze abbrechen, wenn wieder Linie hinter Hindernis erkannt wird
    +        bot_cancel_behaviour(data, bot_solve_maze_behaviour, check_line_sensors);
    +     	break;
    +
    +
    +	case OTHER_SIDE_ON_LINE:  // Bot steht auf anderer Seite des Hindernisses wieder auf der Linie
    +	    // hier muss der bot in die vom Hindernis wegweisende Richtung ausgerichtet werden
    +	    // da er links um den Stein gegangen ist, blickt er entweder etwas Richtung Stein oder fast parallel;
    +	    //einfach links wegdrehen, um ungefaehr die richtige Richtung einzunehmen
    +
    +  	    //Sicherheit einbauen, dass er beim sich auf Linie Ausrichten und Hindernis sehen nicht wieder cancelt
    +  	    //Pos merken und erst wieder nach gewissem Abstand Hinderniserkennung mit Abbruch Linienfolger erlauben
    +  	    oldpos.x=x_pos;
    +  	    oldpos.y=y_pos;
    +  	    line_state_enh=TURN_NEXT_PART;  // naechster Einsprung weiterdrehen mit Cancelueberwachung
    +		bot_turn(data,40);              // etwas links drehen
    +		break;
    +
    +      case TURN_NEXT_PART:  // weitere Drehung in richtiger Richtung fuer Linienfolger mit Cancelueberwachung
    +        //zweiter Teil der Drehung, hier aber Abbruch wenn linker Sensor auf Linie, da dies dann optimal fuer den Linienfolger ist
    +		bot_turn(data,20);
    +		bot_cancel_behaviour(data, bot_turn_behaviour, check_left_line_sensor); // Abbruch Drehung wenn optimal fuer Linienfolger
    +		line_state_enh=SEARCH_LINE;  // naechster Eintrittspunkt
    +		break;
    +
    +	case SEARCH_LINE:  //etwas vorfahren, um optimale Position fuer Linienfolger zu erreichen, falls er noch nicht optimal steht
    +        if (!check_left_line_sensor()) {
    +          bot_goto_dist(data,50,1);  // etwas vor bis nur links Linie erkannt wird, dann optimal fuer Linienfolger
    +          bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_left_line_sensor); // Vorfahren abbrechen wenn optimal fuer Linienfolger
    +        }
    + 		//eigentlich wurde hier alles getan, um den bot optimal fuer den Linienfolger hinzusetezn in die richtige Richtung nach Hindernisumrundung; mit dieser
    + 		//Hoffnung gehts weiter mit dem Linienfolger
    +		line_state_enh=START_LINE_ENH;  // naechster Einsprung->Linienfolger geht von vorn los
    +		break;
    +
    +	default:
    +		return_from_behaviour(data);
    +		break;
    +	}
    +
    +}
    +
    +/*! Botenverhalten fuer den erweiterten Linienfolger
    + * @param *caller Verhaltensdatensatz des Aufrufers
    + */
    +void bot_follow_line_enhanced(Behaviour_t * caller) {
    +	switch_to_behaviour(caller, bot_follow_line_enhanced_behaviour, NOOVERRIDE);
    +	line_state_enh=0;
    +	oldpos.x=0;
    +	oldpos.y=0;
    +}
    +
    +#endif
    +
     #endif	// BEHAVIOUR_FOLLOW_LINE_AVAILABLE
    Index: C:/eclipse/workspace/ct-Bot/Changelog.txt
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/Changelog.txt	(revision 1552)
    +++ C:/eclipse/workspace/ct-Bot/Changelog.txt	(working copy)
    @@ -1,5 +1,7 @@
     CHANGELOG fuer c't-Bot
     ======================
    +2009-02-25 Frank Menzel [Menzelfr@xxxxxx]: neues Verhalten behaviour_follow_line_enhanced; Linienfolger um auch mit Unterbrechungen und Hindernissen klarzukommen
    +
     2009-02-24 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Command-Code optimiert
     												 Bugfix fuer HW-SPI und MOUSE_AVAILABLE (#180)
     												 SPI-Code unterstuetzt verschiedene Geschwindigkeiten
    Index: C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h	(revision 1552)
    +++ C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h	(working copy)
    @@ -30,6 +30,7 @@
     #define BEHAVIOUR_SCAN_AVAILABLE	/*!< Gegend scannen vorhanden? */
     //#define BEHAVIOUR_SOLVE_MAZE_AVAILABLE	/*!< Wandfolger vorhanden? */
     #define BEHAVIOUR_FOLLOW_LINE_AVAILABLE	/*!< Linienfolger vorhanden? */
    +#define BEHAVIOUR_FOLLOW_LINE_ENHANCED_AVAILABLE /*!< erweiterter Linienfolger, der auch mit Unterbrechungen und Hindernis klarkommt */

     #define BEHAVIOUR_SERVO_AVAILABLE 	/*!< Kontrollverhalten fuer die Servos */

    @@ -125,6 +126,13 @@
     	#undef BEHAVIOUR_DRIVE_STACK_AVAILABLE
     #endif

    +#ifdef BEHAVIOUR_FOLLOW_LINE_ENHANCED_AVAILABLE
    +  #define BEHAVIOUR_FOLLOW_LINE_AVAILABLE
    +  #define BEHAVIOUR_GOTO_POS_AVAILABLE
    +  #define BEHAVIOUR_CANCEL_BEHAVIOUR_AVAILABLE
    +  #define BEHAVIOUR_SOLVE_MAZE_AVAILABLE
    +#endif
    +
     #ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE
     	#define BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE
     	#define BEHAVIOUR_TURN_AVAILABLE
    Index: C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_follow_line.h
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_follow_line.h	(revision 1552)
    +++ C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_follow_line.h	(working copy)
    @@ -51,5 +51,19 @@
      * @param *caller Verhaltensdatensatz des Aufrufers
      */
     void bot_follow_line(Behaviour_t * caller);
    +
    +
    +/*! erweiterter Linienfolger, der auch Linienunterbrechungen und Hindernisse handhabt, waehrend der Bot die Linie verfolgt;
    + *  die Linienunterbrechung darf nur relativ klein sein (~3cm), so dass sich beim drehen am Ende der Linie der rechte Abgrundsensor
    + *  ueber den Neubeginn der unterbrochenen Linie drehn muss
    + *  @param *data	Verhaltensdatensatz
    + */
    +void bot_follow_line_enhanced_behaviour(Behaviour_t * data);
    +
    +/*! Botenverhalten fuer den erweiterten Linienfolger
    + * @param *caller Verhaltensdatensatz des Aufrufers
    + */
    +void bot_follow_line_enhanced(Behaviour_t * caller);
    +
     #endif	// BEHAVIOUR_FOLLOW_LINE_AVAILABLE
     #endif	/*BEHAVIOUR_FOLLOW_LINE_H_*/
    Index: C:/eclipse/workspace/ct-Bot/ui/rc5.c
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/ui/rc5.c	(revision 1552)
    +++ C:/eclipse/workspace/ct-Bot/ui/rc5.c	(working copy)
    @@ -152,7 +152,9 @@
     			case 4: bot_follow_object(NULL); break;
     		#endif	// BEHAVIOUR_CATCH_PILLAR_AVAILABLE

    -		#if defined BEHAVIOUR_SOLVE_MAZE_AVAILABLE
    +		#if defined BEHAVIOUR_FOLLOW_LINE_ENHANCED_AVAILABLE
    +		    case 5: bot_follow_line_enhanced(NULL); break;
    +		#elif defined BEHAVIOUR_SOLVE_MAZE_AVAILABLE
     			case 5: bot_solve_maze(NULL); break;
     		#elif defined BEHAVIOUR_FOLLOW_LINE_AVAILABLE
     			case 5: bot_follow_line(NULL); break;
    Index: C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.core.prefs
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.core.prefs	(revision 0)
    +++ C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.core.prefs	(revision 0)
    @@ -0,0 +1,3 @@
    +#Wed Feb 25 11:19:09 CET 2009
    +eclipse.preferences.version=1
    +indexerId=org.eclipse.cdt.core.fastIndexer

- Antworten:
  - [ct-bot] Re: erweiterter Linienfolger-neues Verhalten
    - From: Timo Sandmann

- Voriger (Datum): [ct-bot] Updates, Timo Sandmann
- Nächster (Datum): [ct-bot] Re: erweiterter Linienfolger-neues Verhalten, Timo Sandmann
- Voriger (Thread): [ct-bot] Updates, Timo Sandmann
- Nächster (Thread): [ct-bot] Re: erweiterter Linienfolger-neues Verhalten, Timo Sandmann
- Indizes:
  - Datum
  - Thread

- Aktuelles Heft
- Aktuelles Editorial
- Websites aktuell

#### Support

- Hotline & FAQ
- Treiber & BIOS
- Firmenkontakte

#### Download

- Software zu c't
- Software-Verzeichnis
- c't-Projekte
- Testbilder & Vorlagen

#### Magazin

- Heftarchiv
- c't specials
- Leserforum
- c't-Bildmotive
- Schlagseite

#### Info

- Abo & Heft
- Veranstaltungen
- Mediadaten
- Kontakt
- Impressum

Kleinanzeigen und
Veranstaltungen

- Datenschutzhinweis
- Impressum
- Kritik, Anregungen bitte an c't-WWW
- Mediadaten
- Copyright © 2009 Heise Zeitschriften Verlag
- International: The H, The H Security, The H Open Source, heise online Polska, heise Security Polska, heise Open Source Polska, heise Networks Polska