## c't-Projekte - Mailinglisten

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

### Re: [ct-bot] neues Pfadplanungs-Verhalten nach der Map

Absender: Benjamin Benz
Datum: Mi, 02.05.2007 18:00:10
In-reply-to: <000001c78b5d$62de6570$fe78a8c0@mexpnew>
References: <000001c78b5d$62de6570$fe78a8c0@mexpnew>

    Hi,

    erst mal ein großes Danke für den Patch. Ich denke, das geht in die
    richtige Richtung. Hier ein paar Fragen/Anregungen dazu:

    * Warum ist bot_set_destination_behaviour() ein Verhalten. würde es da
    nicht eine normale Funktion genauso tun?

    * Wie wäre es mit etwas aussagekräftigeren Texten auf dem Screen: z.B.
    letzte Pos: XY
    Position Speichen: 3
    Gehe zu letzter Pos: 4
    Print Map: 6

    * evtl. wäre es sinnvoll die erste Position nicht hart per:
    volatile uint16 dest_x_map=(MAP_SIZE*MAP_RESOLUTION/2);
    volatile uint16 dest_y_map=(MAP_SIZE*MAP_RESOLUTION/2);
    sondern über ein bot_set_destination() beim Start zu initialisieren

    * wäre es nicht praktisch bot_set_destination() eine frei wählbare
    Position übergeben zu können?

    * warum sind die Variablen dest_x_map und Co. volatile?

    * warum verwendet show_labmap() nicht die Funktionen aus map.c, die ein
    Karte als Graustufenbild plotten?

    So, das war erstmal, was mir so auf die schnelle auf- und einfiel. Zum
    testen bin ich leider noch fast nicht gekommen.

    MfG Benjamin Benz

    Frank Menzel wrote:
    > Hallo Allerseits,
    > lange hat's gedauert und nun ist's soweit: das neue
    > Pfadplanungs-Verhalten ! Grundsätzlich stimmt das bereits unten Gesagte,
    > jedoch gibt's keine dynamische Liste mehr. Die Pfadplanung erfolgt nur
    > auf der bereits vorhandenen Karte. Ganz ohne Zugriff auf die Sensoren
    > komme ich nun nicht mehr aus und einen eigenen Screen, von dem aus nur
    > die Routinen aufgerufen werden können (Tasten 3/4/6), habe ich auch
    > definiert. Um auf der vorhandenen Map operieren zu können, habe ich den
    > Wertebereich der online-Aktualisierung eingeschränkt, so dass der
    > hoechste Wert +127 für die Markierung der Pfadpunkte benutzt werden kann
    > ohne durch die ständige Freiaktualisierung überschrieben zu werden. Und
    > die "nur" 2cm für das Hindernisumfeld habe ich auch erhöhen müssen,
    > damit Abstand gehalten wird. Die Pfadplanung und das Fahren
    > funktionieren auch bei hoher Auflösung. Auf dem echten bot klappt's auch
    > ganz passabel.
    > Als Anhang mal der Patch nach den geltenden Richtlinien zum
    > Selbstschauen und Staunen :-)
    > Hoffe nix übersehen zu haben...
    > Gruß, Frank Menzel
    >
    >
    > -----Ursprüngliche Nachricht-----
    > Von: ct-bot-entwickler-bounces@xxxxxxxxxxxxxxxxx
    > [mailto:ct-bot-entwickler-bounces@xxxxxxxxxxxxxxxxx] Im Auftrag von
    > Frank Menzel
    > Gesendet: Freitag, 9. Februar 2007 22:12
    > An: 'Entwicklung rund um den c't-bot'
    > Betreff: [ct-bot] neues Pfadplanungs-Verhalten nach der erstellten/
    > onlineerstellten Karte
    >
    >
    > Hallo allerseits,
    > ich habe ein Verhalten erstellt, welches nur auf der erstellten Karte
    > operiert. Dieses Verhalten läßt den bot zu einer bestimmten Zielposition
    > innerhalb der Map gehen unter Umgehung der vorhandenen Hindernisse. Zum
    > Startzeitpunkt des Verhaltens wird der Weg bis zum Ziel laut der map und
    > der bisher bekannten Hindernisse geplant und die so gefundenen
    > Map-Punktkoordinaten, welche dynamisch im Speicher angelegt werden, bis
    > zum Erreichen des Ziels angefahren.
    > Doch wie kann nun ein Zielpunkt definiert werden ? Ich habe dazu ein
    > weiteres kurzes Verhalten implementiert, welches bei einem Tastendruck
    > die aktuelle Map-Botposition als nunmehr globales Ziel festlegt. Solange
    > keine andere Koordinate mittels diesem Tastendruck als Ziel festgelegt
    > wird, solange gilt die Startposition als zu erreichendes Ziel. Den bot
    > kann man nun die Gegend erforschen lassen oder zu einer anderen Stelle
    > verfahren. Nach Start des Pfadfolge-Verhaltens (via anderem
    > RC5-Tastendruck) fährt der Bot nunmehr zu seinem vorher festgelgten Ziel
    > unter Umgehung der Hindernisse.
    > Wie gesagt, diese hier implementierten Verhalten greifen in keiner Art
    > und Weise auf die Sensoren zu und trotzdem werden die Hindernisse clever
    > umschifft. Bei völlig unerforschtem Gebiet wird die Karte online
    > erstellt und der Weg zum Ziel an den Hindernissen vorbei berechnet.
    >
    > Leider werde ich die nächsten Wochen wohl keine Zeit mehr haben, mich
    > mit dem Thema weiter auseinanderzusetzen und zu verbessern. Daher
    > veröffentliche ich alles bereits zu diesem Zeitpunkt als Patch auf die
    > aktuelle CVS-Version.
    > Ich finde es echt spannend, wie der bot seinen eigenen Weg zum Ziel
    > plant und dann dorthin fährt, ohne direkten Zugriff auf die Sensoren zu
    > haben in diesem Verhalten.
    > Implementiert habe ich es nach der Potenzialfeldmethode, d.h.
    > grundsätzlich wird zum Ziel hin das Potenzial kleiner und umso näher ein
    > Hindernis ist, umso größer ist dort das Potenzial. Aus der Summe der
    > anziehenden und abstossenden Potenziale wird der nächste anzufahrende
    > Punkt ermittelt und denjenigen genommen, dessen Pot. am geringsten ist.
    > Einige Voraussetzungen habe ich noch grundsätzlicher Art schaffen
    > müssen: So ist ja jetzt der Screen 2 belegt für die Verhaltensanzeige
    > mit den dafür benutzten Tastenfunktionen. In allen anderen Screens
    > gelten dieselben anderen Tastenfunktionen. Für die hier benutzten
    > Map-Routinen habe ich Screen 1 (Green) verwendet und die
    > Tastenfunktionen sonst unverändert gelassen. D.h. nur im Screen 1 kann
    > man die Map-Routinen aufrufen ! So wird z.B. mittels Taste 3 die
    > aktuelle Botposition als globales Ziel festgelegt und die Taste 4 läßt
    > den bot von irgendwo zu diesem Ziel hinfahren. Die Taste 5 gibt nur die
    > Koordinaten aller Punkte auf dem Weg zum Ziel aus und die 6 zeigt bei
    > geringer Auflösung die Werte der Karte mit Hindernissen (Kennung 1) an
    > mit der Botposition (Kennung 3).
    > Da ich den bot oft anhalte mittels des Not-Aus Verhaltens, werden ja
    > alle Verhalten bis Prio 200 deaktiviert, so auch das
    > on_the_fly-Verhalten, welches grundlegend für diese Map-Routinen ist.
    > Die Prio habe ich deswegen auf einen Wert > 200 angehoben, um die
    > Aktivierung zu erhalten.
    > Entscheidend ist hier auch die Wahl der Auflösung. Die besten Ergebnisse
    > habe ich bei MCU-Auflösung gemacht, da damit die Zwischenzielpunkte in
    > nicht zu geringem Abstand generiert werden. Bei dieser Auflösung
    > operiere ich direkt mit den jeweiligen Nachbar-Mapkoordinaten. Bei hoher
    > Auflösung wird mittels Umkreis um den zu untersuchenden Punkt die
    > nächste anzufahrende Map-Koordinate ermittelt. Hierbei ist ebenfalls
    > wichtig, daß das zu aktualisierende Umfeld eines Hindernisses nicht zu
    > klein gewählt wird (Define MAP_RADIUS), damit Abstand zum Hindernis
    > gehalten wird. Auf die Ergebnisliste wird keine weitere Routine
    > angewandt, so daß bei höheren Auflösungen zu viele Zwischenzielpunkte
    > erzeugt werden. Hier könnte sicher noch eine Datenreduktion erfolgen.
    > Für den PC habe ich eine Map-Größe von nicht nur 4 gewählt, da 2 m in
    > alle Richtungen hier doch etwas wenig sind.
    > Ich muß hier noch erwähnen, daß mein Versuch dies auf dem echten Bot
    > auszuprobieren, bisher gescheitert ist. Wegen Platzproblemen mußte ich
    > einiges anderes Weglassen. Sobald der Code bei mir drauf war, haben alle
    > LED's geblinkt und nix weiter mehr möglich gewesen. Vielleicht geht's ja
    > bei jemand anderem oder ich habe aus Zeitmangel was übersehen. Es kommt
    > auch vor, daß kein Weg gefunden wird, was verschiedene Ursachen haben
    > kann. Den Bot in diesem Fall etwas verfahren und erneut mit 4 das
    > Verhalten starten.
    > Bei all meinen Versuchen ist mir aufgefallen, daß scheinbar manchmal das
    > bot_turn-Verhalten einfach nicht mehr losläuft. Ursache ist scheinbar
    > das Wand-Notverhalten, welches in zu nahem Abstand das
    > bot_turn-Verhalten blockiert. Dieses Verhalten schalte ich bei höherer
    > Auflösung nun aus. Bei seiner Standort-Selbsterkennung scheint der bot
    > auch manchmal durcheinander zu geraten (nicht nur nach einem
    > Hängenbleiben), was auch unabhängig von meinen Routinen ist.
    >
    > Ich hoffe, ich bin nicht der einzige, der diese Routinen anwendbar
    > findet. Aber selbst nur auf dem PC ist die Pfadplanung und
    > Pfadverfolgung sehr schön anzusehen. Man kann viel Zeit damit
    > verbringen, die ganzen Parameter noch zu verändern und den bot damit
    > laufen zu lassen. Leider habe ich diese Zeit in den kommenden Wochen
    > nicht mehr.
    >
    >
    >
    > Mit freundlichem Gruß
    > Frank Menzel
    >
    >
    >
    > ------------------------------------------------------------------------
    >
    > Index: .cdtbuild
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/.cdtbuild,v
    > retrieving revision 1.9
    > diff -u -r1.9 .cdtbuild
    > --- .cdtbuild	27 Feb 2007 23:45:52 -0000	1.9
    > +++ .cdtbuild	30 Apr 2007 19:39:22 -0000
    > @@ -10,8 +10,8 @@
    >  <option id="gnu.c.compiler.option.include.paths.387475860" superClass="gnu.c.compiler.option.include.paths" valueType="includePath">
    >  <listOptionValue builtIn="false" value="&quot;${ProjDirPath}&quot;"/>
    >  <listOptionValue builtIn="false" value="&quot;${ProjDirPath}/include&quot;"/>
    > -<listOptionValue builtIn="false" value="&quot;C:\Programme\MinGW\include&quot;"/>
    > -<listOptionValue builtIn="false" value="&quot;C:\Programme\pthreads\pthreads.2&quot;"/>
    > +<listOptionValue builtIn="false" value="&quot;C:\MinGW\include&quot;"/>
    > +<listOptionValue builtIn="false" value="&quot;C:\pthreads\pthreads.2&quot;"/>
    >  </option>
    >  <option id="gnu.c.compiler.option.preprocessor.def.symbols.1823712582" superClass="gnu.c.compiler.option.preprocessor.def.symbols" valueType="definedSymbols">
    >  <listOptionValue builtIn="false" value="PC"/>
    > @@ -27,8 +27,8 @@
    >  <listOptionValue builtIn="false" value="pthreadGC2"/>
    >  </option>
    >  <option id="gnu.c.link.option.paths.142594843" superClass="gnu.c.link.option.paths" valueType="stringList">
    > -<listOptionValue builtIn="false" value="&quot;C:\Programme\pthreads\Pre-built.2\lib&quot;"/>
    > -<listOptionValue builtIn="false" value="&quot;C:\Programme\MinGW\lib&quot;"/>
    > +<listOptionValue builtIn="false" value="&quot;C:\pthreads\Pre-built.2\lib&quot;"/>
    > +<listOptionValue builtIn="false" value="&quot;C:\MinGW\lib&quot;"/>
    >  </option>
    >  <option id="gnu.c.link.option.noshared.1812776571" superClass="gnu.c.link.option.noshared" value="true" valueType="boolean"/>
    >  </tool>
    > Index: Changelog.txt
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/Changelog.txt,v
    > retrieving revision 1.142
    > diff -u -r1.142 Changelog.txt
    > --- Changelog.txt	27 Mar 2007 16:26:27 -0000	1.142
    > +++ Changelog.txt	30 Apr 2007 19:39:23 -0000
    > @@ -1,5 +1,9 @@
    >  CHANGELOG fuer c't-Bot
    >  ======================
    > +2007-04-30  Frank Menzel(menzelfr@xxxxxx) Map-Routinen mit Pfadplanung; dazu neuen Screen zum Aufruf der Map-Verhalten definiert; Taste 3 setzt den aktuellen Bot-Standort als Ziel und kann
    > +                        nun von einer beliebigen Position aus direkt zielgerichtet angefahren werden (Taste 4) unter Umgehung der in der Map bereits eingetragenen Hindernisse; Taste 6 gibt
    > +                        die Punkteliste laut Pfadplaner zum Ziel auf die Konsole aus
    > +
    >  2007-03-27 Benjamin Benz [bbe@xxxxxxxx]: Makefile Ã¼berarbeitet
    >
    >  2007-03-05 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Bugfix fuer Remote-Display (falsche Cursorposition, falls nicht vor jedem display_printf() ein display_cursor() efolgte)
    > Index: ct-Bot.h
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/ct-Bot.h,v
    > retrieving revision 1.38
    > diff -u -r1.38 ct-Bot.h
    > --- ct-Bot.h	22 Feb 2007 18:02:10 -0000	1.38
    > +++ ct-Bot.h	30 Apr 2007 19:39:24 -0000
    > @@ -33,7 +33,9 @@
    >  /************************************************************
    >  * Module switches, to make code smaller if features are not needed
    >  ************************************************************/
    > -#define LOG_CTSIM_AVAILABLE		/*!< Logging ueber das ct-Sim (PC und MCU) */
    > +#ifdef PC
    > +  #define LOG_CTSIM_AVAILABLE		/*!< Logging ueber das ct-Sim (PC und MCU) */
    > +#endif
    >  //#define LOG_DISPLAY_AVAILABLE		/*!< Logging ueber das LCD-Display (PC und MCU) */
    >  //#define LOG_UART_AVAILABLE		/*!< Logging ueber UART (NUR fuer MCU) */
    >  //#define LOG_STDOUT_AVAILABLE 		/*!< Logging auf die Konsole (NUR fuer PC) */
    > @@ -50,7 +52,9 @@
    >  //#define TIME_AVAILABLE		/*!< Gibt es eine Systemzeit im s und ms? */
    >
    >  #define DISPLAY_AVAILABLE	/*!< Display for local control */
    > -#define DISPLAY_REMOTE_AVAILABLE /*!< Sende LCD Anzeigedaten an den Simulator */
    > +#ifdef PC
    > +  #define DISPLAY_REMOTE_AVAILABLE /*!< Sende LCD Anzeigedaten an den Simulator */
    > +#endif
    >  #define MEASURE_MOUSE_AVAILABLE			/*!< Geschwindigkeiten werden aus den Maussensordaten berechnet */
    >  //#define MEASURE_COUPLED_AVAILABLE		/*!< Geschwindigkeiten werden aus Maus- und Encoderwerten ermittelt und gekoppelt */
    >
    > @@ -72,7 +76,7 @@
    >
    >  #define BEHAVIOUR_AVAILABLE /*!< Nur wenn dieser Parameter gesetzt ist, exisitiert das Verhaltenssystem */
    >
    > -//#define MAP_AVAILABLE /*!< Aktiviere die Kartographie */
    > +#define MAP_AVAILABLE /*!< Aktiviere die Kartographie */
    >
    >  //#define SPEED_CONTROL_AVAILABLE /*!< Aktiviert die Motorregelung */
    >  //#define UPDATE_PWM_TABLE		/*!< Aktualisiert die PWM-Lookup-Table regelmaessig */
    > @@ -82,8 +86,8 @@
    >
    >  //#define SRF10_AVAILABLE		/*!< Ultraschallsensor SRF10 vorhanden */
    >
    > -#define MMC_AVAILABLE			/*!< haben wir eine MMC/SD-Karte zur Verfuegung */
    > -#define MINI_FAT_AVAILABLE		/*!< koennen wir sektoren in FAT-systemen finden */
    > +//#define MMC_AVAILABLE			/*!< haben wir eine MMC/SD-Karte zur Verfuegung */
    > +//#define MINI_FAT_AVAILABLE		/*!< koennen wir sektoren in FAT-systemen finden */
    >  //#define MMC_VM_AVAILABLE		/*!< Virtual Memory Management mit MMC / SD-Card oder PC-Emulation */
    >
    >  // Achtung, Linkereinstellungen anpassen !!!!! (siehe Documentation/Bootloader.html)!
    > Index: map.c
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/map.c,v
    > retrieving revision 1.4
    > diff -u -r1.4 map.c
    > --- map.c	22 Feb 2007 18:02:10 -0000	1.4
    > +++ map.c	30 Apr 2007 19:39:25 -0000
    > @@ -66,12 +66,15 @@
    >  #define MAP_STEP_FREE		2	/*!< Um diesen Wert wird ein Feld inkrementiert, wenn es als frei erkannt wird */
    >  #define MAP_STEP_OCCUPIED	10	/*!< Um diesen Wert wird ein Feld dekrementiert, wenn es als belegt erkannt wird */
    >
    > -#define MAP_RADIUS			10	/*!< Umkreis einen Messpunkt, der als Besetzt aktualisiert wird (Streukreis) [mm]*/
    > +#define MAP_RADIUS			50	/*!< Umkreis einen Messpunkt, der als Besetzt aktualisiert wird (Streukreis) [mm]*/
    >  #define MAP_RADIUS_FIELDS	(MAP_RESOLUTION*MAP_RADIUS/1000)	/*!< Umkreis einen Messpunkt, der als Besetzt aktualisiert wird (Streukreis) [Felder]*/
    >
    >  #define MAP_PRINT_SCALE				/*!< Soll das PGM eine Skala erhalten */
    >  #define MAP_SCALE	(MAP_RESOLUTION/2)	/*!< Alle wieviel Punkte kommt wein Skalen-Strich */
    >
    > +#define FREE_BOUNDERY 125   /*!< Frei Aktualisierung nur bis zu diesem Wert wegen Pfadplanung */
    > +#define FREEBOUND  (FREE_BOUNDERY-MAP_STEP_FREE) // Grenze wird nicht ueberschritten
    > +
    >  #ifdef SHRINK_MAP_ONLINE
    >  	uint16 map_min_x=MAP_SIZE*MAP_RESOLUTION/2; /*!< belegter Bereich der Karte [Kartenindex]: kleinste X-Koordinate */
    >  	uint16 map_max_x=MAP_SIZE*MAP_RESOLUTION/2; /*!< belegter Bereich der Karte [Kartenindex]: groesste X-Koordinate */
    > @@ -327,7 +330,8 @@
    >   * @param y y-Ordinate der Karte (nicht der Welt!!!)
    >   */
    >  void map_update_free (uint16 x, uint16 y) {
    > -  map_update_field(x,y,MAP_STEP_FREE);
    > +  if (map_get_field(x,y) < FREEBOUND)  // Grenzwert darf nicht ueberschritten werden
    > +    map_update_field(x,y,MAP_STEP_FREE);
    >  }
    >
    >  /*!
    > Index: .settings/org.eclipse.cdt.managedbuilder.core.prefs
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs,v
    > retrieving revision 1.7
    > diff -u -r1.7 org.eclipse.cdt.managedbuilder.core.prefs
    > --- .settings/org.eclipse.cdt.managedbuilder.core.prefs	8 Jul 2006 02:36:15 -0000	1.7
    > +++ .settings/org.eclipse.cdt.managedbuilder.core.prefs	30 Apr 2007 19:39:25 -0000
    > @@ -1,4 +1,4 @@
    > -#Fri Apr 28 20:32:38 CEST 2006
    > +#Mon Apr 30 19:47:47 CEST 2007
    >  eclipse.preferences.version=1
    >  environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n
    >  environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1150677647=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n
    > @@ -10,7 +10,7 @@
    >  environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n
    >  environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.2039935845=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n
    >  environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.release.1181183675=<?xml version\="1.0" encoding\="UTF-8"?>\r\n<environment>\r\n<variable name\="LIBRARY_PATH" operation\="remove"/>\r\n</environment>\r\n
    > -environment/project=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable delimiter\="/" name\="${ProjDirPath}" operation\="replace" value\="C\:\\Dokumente und Einstellungen\\pek\\Eigene Dateien\\ctSim_2006\\workspace\\ct-Bot"/>\n</environment>\n
    > +environment/project=<?xml version\="1.0" encoding\="UTF-8"?>\r\n<environment>\r\n<variable delimiter\="/" name\="${ProjDirPath}" operation\="replace" value\="C\:\\Dokumente und Einstellungen\\pek\\Eigene Dateien\\ctSim_2006\\workspace\\ct-Bot"/>\r\n</environment>\r\n
    >  environment/project/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\r\n<environment/>\r\n
    >  environment/project/cdt.managedbuild.config.gnu.exe.debug.1150677647=<?xml version\="1.0" encoding\="UTF-8"?>\r\n<environment/>\r\n
    >  environment/project/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="PATH" operation\="append" value\="\:/usr/local/avr/bin"/>\n</environment>\n
    > Index: .settings/org.eclipse.core.runtime.prefs
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/.settings/org.eclipse.core.runtime.prefs,v
    > retrieving revision 1.1
    > diff -u -r1.1 org.eclipse.core.runtime.prefs
    > --- .settings/org.eclipse.core.runtime.prefs	15 Jan 2007 11:58:42 -0000	1.1
    > +++ .settings/org.eclipse.core.runtime.prefs	30 Apr 2007 19:39:25 -0000
    > @@ -1,4 +1,4 @@
    > -#Mon Jan 15 12:17:03 CET 2007
    > +#Mon Apr 30 19:56:01 CEST 2007
    >  content-types/enabled=true
    >  content-types/org.eclipse.cdt.core.cHeader/file-extensions=h
    >  content-types/org.eclipse.cdt.core.cSource/file-extensions=c,S
    > Index: bot-logic/bot-logik.c
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/bot-logic/bot-logik.c,v
    > retrieving revision 1.4
    > diff -u -r1.4 bot-logik.c
    > --- bot-logic/bot-logik.c	22 Feb 2007 18:02:10 -0000	1.4
    > +++ bot-logic/bot-logik.c	30 Apr 2007 19:39:26 -0000
    > @@ -136,6 +136,20 @@
    >  	#endif
    >
    >
    > +    // Verhalten, um laut Map zu einem bestimmten Ziel zu fahren
    > +    #ifdef BEHAVIOUR_BORDER_MAP_AVAILABLE
    > +      insert_behaviour_to_list(&behaviour, new_behaviour(199, bot_set_border_in_map_behaviour,ACTIVE));
    > +    #endif
    > +
    > +    // Verhalten, um laut Map zu einem bestimmten Ziel zu fahren
    > +    #ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    > +        insert_behaviour_to_list(&behaviour, new_behaviour(251, bot_set_border_in_map_behaviour,ACTIVE));
    > +        insert_behaviour_to_list(&behaviour, new_behaviour(139, bot_path_bestfirst_behaviour,INACTIVE));
    > +	    insert_behaviour_to_list(&behaviour, new_behaviour(137, bot_set_destination_behaviour,INACTIVE));
    > +	    insert_behaviour_to_list(&behaviour, new_behaviour(137, bot_check_hang_on_behaviour,INACTIVE));
    > + 	    insert_behaviour_to_list(&behaviour, new_behaviour(135, bot_gotoxy_behaviour_map,INACTIVE));
    > +    #endif
    > +
    >  	#ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE
    >  		insert_behaviour_to_list(&behaviour, new_behaviour(44, bot_catch_pillar_behaviour,INACTIVE));
    >  	#endif
    > Index: include/map.h
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/include/map.h,v
    > retrieving revision 1.3
    > diff -u -r1.3 map.h
    > --- include/map.h	15 Jan 2007 11:07:33 -0000	1.3
    > +++ include/map.h	30 Apr 2007 19:39:26 -0000
    > @@ -93,6 +93,21 @@
    >  int8 map_get_field (uint16 x, uint16 y);
    >
    >  /*!
    > + * Setzt den Wert eines Feldes auf den angegebenen Wert
    > + * @param x x-Ordinate der Karte (nicht der Welt!!!)
    > + * @param y y-Ordinate der Karte (nicht der Welt!!!)
    > + * @param value neuer wert des Feldes (> 0 heisst frei, <0 heisst belegt
    > + */
    > +void map_set_field(uint16 x, uint16 y, int8 value);
    > +
    > +/*!
    > + * markiert ein Feld als belegt -- drueckt den Feldwert etwas mehr in Richtung "belegt"
    > + * @param x x-Ordinate der Karte (nicht der Welt!!!)
    > + * @param y y-Ordinate der Karte (nicht der Welt!!!)
    > + */
    > +void map_update_occupied (uint16 x, uint16 y);
    > +
    > +/*!
    >   * liefert den Wert eines Feldes
    >   * @param x x-Ordinate der Welt
    >   * @param y y-Ordinate der Welt
    > Index: include/bot-logic/available_behaviours.h
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/include/bot-logic/available_behaviours.h,v
    > retrieving revision 1.2
    > diff -u -r1.2 available_behaviours.h
    > --- include/bot-logic/available_behaviours.h	15 Jan 2007 11:07:34 -0000	1.2
    > +++ include/bot-logic/available_behaviours.h	30 Apr 2007 19:39:26 -0000
    > @@ -26,12 +26,17 @@
    >
    >  #define BEHAVIOUR_REMOTECALL_AVAILABLE /*!< Nehmen wir Remote-kommandos entgegen?*/
    >
    > +#define BEHAVIOUR_BORDER_MAP_AVAILABLE  /*!< Abgrund-Notverhalten mit Map-Aktualisierung*/
    > +#define BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE /*!< Fahren zu einem Ziel nach Pfadplanung*/
    > +
    >  /* Aufgrund einer ganzen reihe von Abhaengigkeiten sollte man beim Versuch Speicher
    >   * zu sparen, zuerst mal bei den Hauptverhalten ausmisten, sonst kommen die
    >   * Unterverhalten durch die Hintertuer wieder rein
    >   */
    >  #ifndef MAP_AVAILABLE
    >  	#undef BEHAVIOUR_SCAN_AVAILABLE
    > +	#undef BEHAVIOUR_BORDER_MAP_AVAILABLE
    > +	#undef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    >  #endif
    >
    >  #ifdef BEHAVIOUR_GOTOXY_AVAILABLE
    > @@ -93,6 +98,8 @@
    >  #include "bot-logic/behaviour_catch_pillar.h"
    >
    >  #include "bot-logic/remote_calls.h"
    > +
    > +#include "bot-logic/behaviour_map_go_destination.h"
    >
    >  #endif
    >  #endif /*AVAILABLE_BEHAVIOURS_H_*/
    > Index: include/bot-logic/behaviour_gotoxy.h
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/include/bot-logic/behaviour_gotoxy.h,v
    > retrieving revision 1.1
    > diff -u -r1.1 behaviour_gotoxy.h
    > --- include/bot-logic/behaviour_gotoxy.h	6 Nov 2006 12:32:13 -0000	1.1
    > +++ include/bot-logic/behaviour_gotoxy.h	30 Apr 2007 19:39:32 -0000
    > @@ -30,6 +30,7 @@
    >
    >  #include "bot-logic/bot-logik.h"
    >
    > +
    >  #ifdef BEHAVIOUR_GOTOXY_AVAILABLE
    >  /*!
    >   * Das Verhalten faehrt von der aktuellen Position zur angegebenen Position (x/y)
    > @@ -45,6 +46,14 @@
    >   * @param y Y-Ordinate an die der Bot fahren soll
    >   */
    >  void bot_gotoxy(Behaviour_t *caller, float x, float y);
    > +
    > +/*!
    > + * Berechnung der benoetigten Drehung
    > + * @param xDiff
    > + * @param yDiff
    > + */
    > +int16 bot_gotoxy_calc_turn(float xDiff, float yDiff);
    > +
    >  #endif
    >
    >  #endif /*BEHAVIOUR_GOTOXY_H_*/
    > Index: ui/available_screens.h
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/ui/available_screens.h,v
    > retrieving revision 1.1
    > diff -u -r1.1 available_screens.h
    > --- ui/available_screens.h	22 Feb 2007 18:02:10 -0000	1.1
    > +++ ui/available_screens.h	30 Apr 2007 19:39:32 -0000
    > @@ -42,6 +42,8 @@
    >  //#define RESET_INFO_DISPLAY_AVAILABLE	/*!< Zeigt Informationen ueber Resets an */
    >  #define RAM_DISPLAY_AVAILABLE			/*!< Ausgabe des freien RAMs */
    >
    > +#define DISPLAY_MAP_GO_DESTINATION      /*!< Steuerung Map-Verhalten auf diesem Screen */
    > +
    >  #ifndef SPEED_CONTROL_AVAILABLE
    >  	#undef DISPLAY_REGELUNG_AVAILABLE
    >  #endif
    > Index: ui/gui.c
    > ===================================================================
    > RCS file: /ctbot/ct-Bot/ui/gui.c,v
    > retrieving revision 1.1
    > diff -u -r1.1 gui.c
    > --- ui/gui.c	22 Feb 2007 18:02:10 -0000	1.1
    > +++ ui/gui.c	30 Apr 2007 19:39:32 -0000
    > @@ -106,6 +106,12 @@
    >  	#ifdef RAM_DISPLAY_AVAILABLE
    >  		register_screen(&ram_display);
    >  	#endif
    > +	#ifdef DISPLAY_MAP_GO_DESTINATION
    > +	  #ifdef MAP_AVAILABLE
    > +		register_screen(&mapgo_display);
    > +      #endif
    > +	#endif
    > +
    >  }
    >
    >  #endif	// DISPLAY_AVAILABLE
    > Index: bot-logic/behaviour_map_go_destination.c
    > ===================================================================
    > RCS file: bot-logic/behaviour_map_go_destination.c
    > diff -N bot-logic/behaviour_map_go_destination.c
    > --- /dev/null	1 Jan 1970 00:00:00 -0000
    > +++ bot-logic/behaviour_map_go_destination.c	1 Jan 1970 00:00:00 -0000
    > @@ -0,0 +1,1595 @@
    > +/*
    > + * c't-Bot
    > + *
    > + * This program is free software; you can redistribute it
    > + * and/or modify it under the terms of the GNU General
    > + * Public License as published by the Free Software
    > + * Foundation; either version 2 of the License, or (at your
    > + * option) any later version.
    > + * This program is distributed in the hope that it will be
    > + * useful, but WITHOUT ANY WARRANTY; without even the implied
    > + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
    > + * PURPOSE. See the GNU General Public License for more details.
    > + * You should have received a copy of the GNU General Public
    > + * License along with this program; if not, write to the Free
    > + * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
    > + * MA 02111-1307, USA.
    > + *
    > + */
    > +
    > +/*! @file 	behaviour_map_go_destination.c
    > + *  @brief 	Bot faehrt nach der erstellten Karte (bzw. wird online erstellt) zu einer bestimmten
    > + *          Zielkoordinate und umgeht durch vorheriger Pfadplanung nach der Potenzialfeldmethode
    > + *          vorhandene Hindernisse; die Zielkoordinate ist zu Beginn die
    > + *          Startposition und kann mittels Tastendruck(3)im gesonderten Verhalten auf die aktuelle
    > + *          Botposition geaendert werden; einfach den Bot im Raum umherfahren und bei Druecken der 4
    > + *          findet der bot den Weg zur Startposition zurueck (oder falls 3 betaetigt wurde zu dieser
    > + *          neuen Zielposition )
    > +
    > + *
    > + *  @author 	Frank Menzel (-alias achiem -Menzelfr@xxxxxxx)
    > + *  @date 	    30.04.07
    > +*/
    > +
    > +
    > +#include "bot-logic/bot-logik.h"
    > +#include "map.h"
    > +#include <stdlib.h>
    > +#include <math.h>
    > +#include "ui/available_screens.h"
    > +#include "display.h"
    > +#include "rc5.h"
    > +#include "rc5-codes.h"
    > +#include "log.h"
    > +
    > +#ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    > +
    > +/*!< naechstes anzufahrendes Ziel laut Pfadplanung
    > + */
    > +uint16 next_x=0;
    > +uint16 next_y=0;
    > +
    > +/*!< Zwischenzielkoordinaten X Y des xy-Fahrverhaltens
    > + */
    > +static uint16 target_x=0;
    > +static uint16 target_y=0;
    > +
    > +/*! Umkreisermittlung fuer Pfadfindung; im Umkreis ab einer Koord der je nach Aufloesung berechneten Mapfelder
    > + *  wichtig ist zur Hindernisumfahrung auch das Setzen der Hinderniswahrscheinlichkeit auf Radiusumfeld und darf nicht zu klein sein
    > + */
    > +#define MAP_RADIUS_GODEST			  120 // ca. der Radiuskreis; bei hoeherem Wert wird Ziel bei hoher Res. schneller gefunden
    > +#define MAP_RADIUS_FIELDS_GODEST	  (MAP_RESOLUTION*MAP_RADIUS_GODEST/1000)	/*!< Umkreis einen Messpunkt, der als Besetzt aktualisiert wird (Streukreis) [Felder]*/
    > +
    > +// auf der Haelfte des Wertes MAP_RADIUS_FIELDS_GODEST wird in map_in_dest Punkt innerhalb des Umkreises gesucht; also innerhalb 4cm Umkreis gilt Punkt als getroffen
    > +#define MAP_RADIUS_FIELDS_GODEST_HALF (MAP_RADIUS_FIELDS_GODEST / 2)
    > +#define MAP_RADIUS_FIELDS_GODEST_HALF_QUAD (MAP_RADIUS_FIELDS_GODEST_HALF * MAP_RADIUS_FIELDS_GODEST_HALF) // halber Quadratradius
    > +
    > +
    > +// Teilerwert zur Potenzialberechnung des attraktiven Potenzials je nach Aufloesung und Suchumkreis
    > +#if MAP_RADIUS_FIELDS_GODEST <= 1  // MCU-Aufloesung
    > +   #define DIV_ATTPOT 1
    > +#else
    > +   #if MAP_RADIUS_FIELDS_GODEST > 9   // sehr hohe Aufloesung
    > +     #define DIV_ATTPOT 1000
    > +   #else
    > +     #define DIV_ATTPOT 10
    > +   #endif
    > +#endif
    > +
    > +#ifdef PC
    > +  #define MAXCOUNT_SAMEPOS 80     // Abbruchcounter, falls bot haengenbleibt
    > +#else
    > +  #define MAXCOUNT_SAMEPOS 800
    > +#endif
    > +
    > +#define MAXCOUNTER 8           // Zielsuche bis zu dieser Tiefe, hier wird Routine spaetestens beendet
    > +
    > +/*! Werte, wie bestimmte Mapfelder angezeigt werden zur Visualisierung bei geringer Aufloesung
    > + */
    > + #define SPACE 0           // Leer- Initialfeld
    > + #define WALL  1           // Wandfeld
    > + #define HOLE  7           // Abgrund, Loch
    > + #define BOTPOS  3         // Botposition selbst
    > + #define PATHNODES  127      // aus Pfadplanung
    > + #define GOAL  127      // Ziel bekommt freieste Feldwahrscheinlichkeit
    > +
    > +/*! Abgrundsensoren 5mm weiter aussen von Distsensoren
    > + */
    > + #define DISTSENSOR_POS_B_SW   (DISTSENSOR_POS_SW + 5)
    > +
    > +/*! globale Zielkoordinaten fuer Pfadplanung; ist zuerst immer das Bot-Map-Startfeld
    > + */
    > +volatile uint16 dest_x_map=(MAP_SIZE*MAP_RESOLUTION/2);
    > +volatile uint16 dest_y_map=(MAP_SIZE*MAP_RESOLUTION/2);
    > +
    > +/*! Zustandsvar der Verhalten nach aussen gezogen, um dieses im Botenverhalten korrekt init. zu koennen;nach Notaus
    > + *  koennte diese sonst einen undef. Zustand haben
    > + */
    > +static uint8 gotoStatexy=0;  // Statusvar des xy-Fahr-Verhaltens
    > +static uint8 statehangon=0;  // Statusvar des Haengon-Verhaltens
    > +
    > +/*! Abbruchzaehler falls bot sich nicht bewegt
    > + */
    > +static uint16 cancelcounter_samepos=0;
    > +
    > +/*! True wenn Border_map_behaviour einen Abgrund erkannt und markiert hatte;
    > + *  muss vom Anwenderprog auf False gesetzt werden und dient zur Erkennung ob Verhalten
    > + *  zugeschlagen hat
    > +*/
    > +static int8 border_behaviour_fired=False;
    > +
    > +
    > +/*! True, wenn vom Haengenbleiben-Verhalten das Haengenbleiben des bots erkannt wurde
    > + *  muss von Anwendung nach Auswertung wieder auf False gesetzt werden
    > + */
    > +static int8 hangon_behaviour_fired=False;
    > +
    > +
    > + /*! Elternposition, d.h. Botposition vor neuer Position
    > + */
    > +static uint16 x_parent=0;
    > +static uint16 y_parent=0;
    > +
    > +
    > +// ============= notwendige Hilfsroutinen fuer die Verhalten =======
    > +
    > +#if MAP_RADIUS_FIELDS_GODEST > 0
    > +
    > +    /*! Map-Umfeldaktualisierung mit einem bestimmten Wert ab einer Position xy mit Radius r bei
    > +     *  hoeherer Aufloesung; z.B. verwendet zur Lochmarkierung in einem Umkreis, Hinderniswahrscheinlichkeit
    > +     *  @param xy Map-Koordinaten
    > +     *  @param radius Radius des Umfeldes
    > +     *  @param value Mapwert; nur eingetragen wenn aktueller Mapwert groesser value ist
    > +     */
    > +	void map_set_value_field_circle(uint16 x, uint16 y, int8 radius, int8 value) {
    > +		int16 dX,dY;
    > +	    uint16 h=radius*radius;
    > +		for(dX = -radius; dX <= radius; dX++){
    > +	      for(dY = -radius; dY <= radius; dY++) {
    > +	           if(dX*dX + dY*dY <= h)	                    // nur innerhalb des Umkreises
    > +	           	 if (map_get_field(x + dX, y + dY)>value)   // Mapwert hoeher Richtung frei ?
    > +	           	 	  map_set_field (x + dX, y + dY,value); // dann Wert eintragen
    > +	      }
    > +		}
    > +	}
    > +
    > +	/*! gibt True zurueck wenn Map-Wert value innerhalb des Umkreises radius von xy liegt sonst False;
    > +	 *  wird verwendet zum Check, ob sich ein Punkt (naechster Pfadpunkt, Loch) innerhalb eines bestimmten
    > +	 *  Umkreises befindet; findet nur Verwendung bei hoeherer Aufloesung
    > +	 *  @param xy Map-Koordinaten
    > +	 *  @param radius Radius des Umfeldes
    > +	 *  @param value Mapwert des Vergleiches
    > +	 *  @return True wenn Wert value gefunden
    > +	 */
    > +	int8 map_get_value_field_circle(uint16 x, uint16 y, int8 radius, int8 value) {
    > +	int16 dX,dY;
    > +	uint16 h=radius*radius;
    > +		for(dX = -radius; dX <= radius; dX++){
    > +	      for(dY = -radius; dY <= radius; dY++) {
    > +	           if(dX*dX + dY*dY <= h)	                    // Vergleich nur innerhalb des Umkreises
    > +	           	 if (map_get_field(x + dX, y + dY)==value)  // Mapwert hat Vergleichswert ?
    > +	           	 	return True; 	           	            // dann Schluss mit True
    > +	      }
    > +		}
    > +		return False;                                       // kein Feldwert ist identisch im Umkreis
    > +	}
    > +
    > +#endif  // hoehere Aufloesung
    > +
    > +/*!
    > + * setzt ein Map-Feld auf einen Wert mit Umfeldaktualisierung; Hindernis wird mit halben Botradius
    > + * eingetragen, da die Pfadpunkte im ganzen Umkreis liegen und nicht ueberschrieben werden duerfen;
    > + * Abgrund/ Loch wird aber auch im ganzen Botradius eingetragen; bei geringer MCU-Aufloesung ohne Umfeld mit
    > + * direktem Eintragen des Wertes auf xy; Umfeld wird dann in Richtung Hindernis gedrueckt
    > + * @param xy  Map-Koordinaten
    > + * @param val im Umkreis einzutragender Wert
    > + */
    > +void map_set_value_occupied (uint16 x, uint16 y, int8 val) {
    > +
    > +  #if MAP_RADIUS_FIELDS_GODEST > 0              // bei hoeherer Aufloesung
    > +	  uint8 r;
    > +	  uint8 maxr;
    > +
    > +	  if (val==-128)
    > +	      maxr=MAP_RADIUS_FIELDS_GODEST;        // Loch wird mit ganzem Botradius eingetragen
    > +	    else
    > +	      maxr=MAP_RADIUS_FIELDS_GODEST_HALF;   // im Normalfall halber Botradius
    > +
    > +	  for (r=1; r<=maxr; r++)                   // in Map mit dem Radius um xy eintragen
    > +	  		map_set_value_field_circle(x,y,r,val);
    > +
    > +  #else                                         // bei geringer MCU-Aufloesung direkt auf xy
    > +  	if (map_get_field(x,y)>val)                 // nur Eintragen wenn aktueller Wert freier ist
    > +  		  map_set_field(x,y,val);               // damit auch -128 Lochwert eingetragen wird
    > +  #endif
    > +
    > +  // Punkt xy nun in Richtung Hindernis druecken mit Umfeld; falls Wert fuer kuenstl. Hindernis erreicht
    > +  // muss Wert aber so bleiben, damit diese Mapfelder bei Neuberechnung der Map-Potenziale erkannt und wieder
    > +  // freigegeben werden koennen zur Neuberuecksichtigung
    > +  if (val != -MAP_ART_HAZARD)
    > +       map_update_occupied(x,y);
    > +}
    > +
    > +
    > +/*!
    > + * ermittelt ob der Wert val innerhalb des Umkreises von xy liegt; bei geringer MCU-Aufloesung direkter
    > + * Vergleich mit xy
    > + * @param xy  Map-Koordinaten
    > + * @param val Vergleichswert
    > + * @return True wenn Wert val gefunden
    > + */
    > +int8 value_in_circle (uint16 x, uint16 y, int8 val) {
    > +
    > +  #if MAP_RADIUS_FIELDS_GODEST > 0
    > +	  uint8 r;
    > +      for (r=1; r<=MAP_RADIUS_FIELDS_GODEST; r++){          // Botradius
    > +	  		if (map_get_value_field_circle(x,y,r,val))      // Schluss sobald Wert erkannt wird
    > +	  		  return True;
    > +	  }
    > +	  return False;                                         // Wert nicht vorhanden
    > +  #else
    > +  	return map_get_field(x,y)==val;  	                    // niedrige Aufloesung, direkter Feldvergleich
    > +  #endif
    > +}
    > +
    > +
    > +
    > +/*! Routine ermittelt ab Hoehe xp yp in Blickrichtung dist mm voraus die Mapkoordinaten XY; verwendet
    > + *  zur Map-Lochmarkierung in Hoehe Abgrundsensoren; kann aber allgemeingueltig verwendet werden um
    > + *  Mapkoordinaten zu erhalten in einem bestimmten Abstand voraus
    > + * @param xp yp Koord vom Mittelpunkt des bots verschoben
    > + * @param h Blickrichtung
    > + * @param X Y berechnete Mapkoordinate im Abstand dist ab Hoehe Abstandssensoren
    > + * @param dist Abstand voraus in mm
    > + */
    > +void get_mappos_dist(float xp, float yp, float h, uint16 dist, uint16 *X, uint16 *Y ) {
    > +
    > +	// Hinderniss, dass der Sensor sieht in Weltkoordinaten
    > +	float PH_x = xp + (DISTSENSOR_POS_FW + dist) * cos(h);
    > +	float PH_y = yp + (DISTSENSOR_POS_FW + dist) * sin(h);
    > +
    > +	// Hinderniss, welches der Sensor sieht in, umgerechnet in Karten-Map-Koordinaten
    > +	*X = world_to_map(PH_x);
    > +	*Y = world_to_map(PH_y);
    > +
    > +}
    > +
    > +/*! markiert die Mapkoordinaten als Loch zum entsprechenden Abgrundsensor
    > + *  xy  bereits berechnete Koordinaten nach links rechts vom Mittelpunkt in Hoehe des Sensors
    > + *  h   Blickrichtung bereits umgerechnet in Bogenmass
    > + */
    > +void update_map_sensor_hole(float x, float y, float h){
    > +
    > +
    > +	uint16 PH_X=0;
    > +	uint16 PH_Y=0;
    > +
    > +    // 0 mm voraus Loch ab Hoehe Abgrundsensoren
    > +	get_mappos_dist(x,y,h,0,&PH_X,&PH_Y);
    > +
    > +    // Syncvar setzen, damit Auswerteverhalten dies erkennt
    > +	border_behaviour_fired=True;
    > +
    > +	// nur wenn noch nicht als Loch gekennzeichnet auf UmgebungskreisLoch vermerken
    > +	if (map_get_field(PH_X,PH_Y) > -128)
    > +			map_set_value_occupied(PH_X,PH_Y,-128);
    > +
    > +	// wichtig fuer hoehere Aufloesung ist, dass Hinderniswahrscheinlichkeit auf
    > +	// Bot-Umkreis gesetzt wird
    > +	map_update_occupied(PH_X,PH_Y);
    > +
    > +}
    > +
    > + /*! eigentliche Aufrufroutine zum Eintragen des Abgrundes in den Mapkoordinaten, wenn
    > +  *  die Abgrundsensoren zugeschlagen haben
    > +  *  @param xy   aktuelle Bot-Koordinaten als Welt- (nicht Map-) Koordinaten
    > +  *  @param head Blickrichtung
    > +  */
    > + void update_map_hole(float x, float y, float head){
    > +
    > +    float h= head * M_PI /180;  // Umrechnung in Bogenmass
    > +
    > +    if (sensBorderL > BORDER_DANGEROUS) {
    > +       //Ort des linken Sensors in Weltkoordinaten (Mittelpunktentfernung)
    > +       float Pl_x = x - (DISTSENSOR_POS_B_SW * sin(h));
    > +	   float Pl_y = y + (DISTSENSOR_POS_B_SW * cos(h));
    > +
    > +	   update_map_sensor_hole(Pl_x,Pl_y,h); // Eintragen des Loches in die Map
    > +    }
    > +
    > +	if (sensBorderR > BORDER_DANGEROUS) {
    > +      //Ort des rechten Sensors in Weltkoordinaten (Mittelpunktentfernung)
    > +      float Pr_x = x + (DISTSENSOR_POS_B_SW * sin(h));
    > +	  float Pr_y = y - (DISTSENSOR_POS_B_SW * cos(h));
    > +
    > +
    > +	  update_map_sensor_hole(Pr_x,Pr_y,h); // Eintragen des Loches in die Map
    > +	}
    > +}
    > +
    > +
    > +/*! Loescht die als frei gekenzeichneten Mapfelder > 0 fuer neues Pfadplanung oder nur die kuenstlich erzeugten Hindernisse
    > + *  beim Entfernen der k. Hind. werden auch die gekennzeichneten Felder fuer den ermittelten Weg wieder geloescht
    > + *  @param clear_only_art_hazards bei True nur die kuenstl. Hindernisse auf 0 sonst die Freifelder
    > + */
    > +void clear_map(int8 clear_only_art_hazards){
    > +
    > +		uint16 x,y;
    > +		int8 tmp;
    > +
    > +         // Mapfelder durchlaufen
    > +		 for (y=map_min_y; y<= map_max_y; y++) {
    > +			  for (x=map_max_x; x>= map_min_x; x--) {
    > +
    > +				tmp=map_get_field(x,y);
    > +
    > +				// Loeschen der kuenstlichen Hindernisse und des berechneten Pfades
    > +				// Im Anschluss daran sollte die urspruengliche Map wieder vorhanden sein, die dann wieder
    > +				// fuer ein neues Ziel herangezogen werden kann
    > +				if (clear_only_art_hazards) {
    > +					if (tmp == -MAP_ART_HAZARD || tmp==PATHNODES ) // Werte > 0 loeschen, Hindernisse bestehen lassen
    > +				    map_set_field(x,y,0);
    > +				}
    > +				else {
    > +				   // Loeschen der Freifelder, vorheriger Pfad dadurch auch geloescht und neue Suche
    > +				  if (tmp > 0 ) // Werte > 0 loeschen, Hindernisse bestehen lassen
    > +				    map_set_field(x,y,0);
    > +				}
    > +			}
    > +		}
    > +	}
    > +
    > +
    > +// ============= eigenstaendige Parallelverhalten zum eigentlichen Fahrverhalten =======
    > +
    > +
    > +/*! Verhalten zum Erkennen und Markieren eines Abgrundes (wenn MAP_AVAILABLE gesetzt) in die Map, falls
    > + *  einer der Abgrundsensoren dies erkennt; es wird in diesem Fall auch etwas zurueckgefahren, wenn
    > + *  BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE gesetzt ist und falls nicht, dann gilt es rueckwaerts zu fahren bis
    > + *  darunter kein Loch mehr erkannt wird (wie im aktuellen Abgrundverhalten auch); Verhalten wird gleich mit ACTIVE
    > + *  als Endlosverhalten gestartet und deaktiviert das aktuell vorhandene Notfallverhalten;
    > + *  --sollte eigentlich generell und allgemein das bisherige Notfallverhalten ersetzen koennen--
    > + *  @param *data der Verhaltensdatensatz
    > + */
    > +void bot_set_border_in_map_behaviour(Behaviour_t *data){
    > +	static uint8 state=0;
    > +
    > +	switch (state){
    > +		case 0:
    > +			if  ( sensBorderL > BORDER_DANGEROUS || sensBorderR > BORDER_DANGEROUS) {
    > +			     #ifdef BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE
    > +			       bot_drive_distance(data,0,-BOT_SPEED_FOLLOW,10);// 10cm rueckwaerts nur bei Hindernis
    > +			     #else
    > +			       // Verhalten bot_drive nicht existent, dann wenigstens nicht ins Loch fahren wie avoid border
    > +			       if (sensBorderL > BORDER_DANGEROUS)
    > +		             speedWishLeft=-BOT_SPEED_NORMAL;
    > +
    > +	               if (sensBorderR > BORDER_DANGEROUS)
    > +		              speedWishRight=-BOT_SPEED_NORMAL;
    > +			     #endif
    > +
    > +                 #ifdef MAP_AVAILABLE
    > +					update_map_hole(x_pos,y_pos,heading);
    > +                 #endif
    > +
    > +			     }
    > +
    > +			break;
    > +		default:
    > +			state=0;
    > +			return_from_behaviour(data);
    > +			break;
    > +	}
    > +}
    > +
    > +
    > +
    > +/*! Verhalten zum Erkennen des Haengenbleibens des Bots; wenn MEASURE_MOUSE_AVAILABLE gesetzt werden Maus- und
    > + *  Encoderdaten miteinander verglichen, sonst nur die Veraenderung der Mapkoordinaten innerhalb einer gewissen
    > + *  Zeitspanne (via counter realisiert) ausgewertet; wird der Bot-Stillstand erkannt, wird etwas rueckwaerts
    > + *  gefahren und die Syncvar hangon_behaviour_fired gesetzt, damit das Haengenbleiben vom Fahrverhalten erkannt
    > + *  wird und reagiert werden kann; koennte eigentlich im bot auch generell als uebergeordnetes Verhalten
    > + *  eingesetzt werden, hier jedoch aktiviert zum Start des Fahrverhaltens selbst und bei Ankunft deaktiviert
    > + *  @param *data der Verhaltensdatensatz
    > + */
    > +void bot_check_hang_on_behaviour(Behaviour_t *data){
    > +
    > +	 #ifndef MEASURE_MOUSE_AVAILABLE
    > +	   static uint16 diffx_last=0;
    > +	   static uint16 diffy_last=0;
    > +	   int16 xDiff;
    > +	   int16 yDiff;
    > +	 #endif
    > +
    > +
    > +
    > +	switch (statehangon){
    > +		case 0:
    > +
    > +		  // Check auf Haengenbleiben mit Rad-Durchdreh-Erkennung mit Mausvergleich
    > +           #ifdef MEASURE_MOUSE_AVAILABLE
    > +             // wenn Maus verfuegbar, so kann ein Haengenbleiben durch Vergleich der Encoder und
    > +             // Mauswerte festgestellt werden, wie auf bot-Webseite schon mal beschrieben
    > +            if  ( !((abs(v_enc_left-v_mou_left) < STUCK_DIFF)    &&
    > +                   (abs(v_enc_right-v_mou_right) < STUCK_DIFF)
    > +                  ))
    > +             {
    > +
    > +           #else
    > +              // ohne Maus wird ein Haengenbleiben daran erkannt, dass sich gewisse Zeit der bot in
    > +              // denselben Mapkoordinaten aufhaelt abzueglich gewisser Toleranz je nach Aufloesung
    > +			 xDiff=target_x-world_to_map(x_pos);
    > +			 yDiff=target_y-world_to_map(y_pos);
    > +			 if ((diffx_last < (fabs(xDiff) +  MAP_RADIUS_FIELDS_GODEST))&& (diffx_last>(fabs(xDiff)- MAP_RADIUS_FIELDS_GODEST)) &&
    > +			     (diffy_last < (fabs(yDiff) +  MAP_RADIUS_FIELDS_GODEST))&& (diffy_last>(fabs(yDiff)- MAP_RADIUS_FIELDS_GODEST))
    > +			    ) {
    > +		   #endif
    > +		   	  cancelcounter_samepos++;
    > +
    > +			  if (cancelcounter_samepos>=MAXCOUNT_SAMEPOS) {
    > +			    cancelcounter_samepos=0;
    > +
    > +			    // Syncvar setzen, dass Verhalten zugeschlagen hat
    > +		        hangon_behaviour_fired = True;
    > +
    > +			    // etwas rueckwaerts fahren;
    > +			    #ifdef BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE
    > +			      bot_drive_distance(data,0,-BOT_SPEED_FOLLOW,10);
    > +			    #endif
    > +
    > +			    break;
    > +			  }
    > +			}
    > +			else
    > +			   cancelcounter_samepos=0;  // wieder auf 0, da Aenderung stattgefunden hat
    > +
    > +			#ifndef MEASURE_MOUSE_AVAILABLE
    > +			  // Differenz vom letzten Durchlauf merken, wenn Mausvergleich nicht aktiv
    > +			  diffx_last=fabs(xDiff);
    > +		      diffy_last=fabs(yDiff);
    > +		    #endif
    > +
    > +			break;
    > +		default:
    > +		    // wird eigentlich nicht erreicht, da Endlosverhalten
    > +			statehangon=0;
    > +			return_from_behaviour(data);
    > +			break;
    > +	}
    > +}
    > +
    > +/*! Botenverhalten zum Starten des Haengenbleiben-Verhaltens
    > + *  @param *caller der Verhaltensdatensatz
    > + */
    > +void bot_check_hang_on(Behaviour_t *caller) {
    > +	statehangon=0;
    > +	cancelcounter_samepos=0;
    > +	switch_to_behaviour(caller, bot_check_hang_on_behaviour,NOOVERRIDE);
    > +}
    > +
    > +
    > +
    > +/*! Verhalten zum Setzen des globalen Zielpunktes dest_x_map und dest_y_map;nach Aktivierung
    > + *  durch das Botenverhalten wird die Botposition ermittelt und in die
    > + *  globale Zielposition gespeichert; das Verhalten wird daraufhin sofort wieder deaktiviert
    > + *  @param *data der Verhaltensdatensatz
    > + */
    > +void bot_set_destination_behaviour(Behaviour_t *data){
    > +	static uint8 state=0;
    > +
    > +	switch (state){
    > +		case 0:
    > +			//Botposition ermitteln und in globale Zielkoordinaten speichern
    > +			dest_x_map = world_to_map(x_pos);
    > +	        dest_y_map = world_to_map(y_pos);
    > +
    > +            #ifdef PC  // platzsparend fuer den echten bot
    > +              LOG_DEBUG(("globales Ziel gesetzt auf Botposition: %1d %1d \n",dest_x_map, dest_y_map));
    > +            #endif
    > +
    > +			state++;  // Schluss im naechsten Status
    > +			break;
    > +		default:
    > +			state=0;
    > +			return_from_behaviour(data);
    > +			break;
    > +	}
    > +}
    > +
    > +/*! Botenverhalten zum Setzen der Zielkoordinaten auf aktuelle Botposition
    > + *  @param *caller der Verhaltensdatensatz
    > + */
    > +void bot_set_destination(Behaviour_t *caller) {
    > +	switch_to_behaviour(caller, bot_set_destination_behaviour,NOOVERRIDE);
    > +}
    > +
    > +
    > +
    > +
    > +// ===== notwendige Routinen zur Ermittlung der Potenziale fuer das eigentliche Fahrverhalten =======
    > +
    > +/*! Ermittlung des Abstandes zwischen 2 Koordinaten; verwendet zur Potenzial-Abstandsermittlung zum Zielpunkt,
    > + *  um eine heuristische Schaetzung vorzunehmen
    > + * @param xs ys Map-Koordinaten des zu untersuchenden Punktes
    > + * @param xd yd Map-Koordinaten des Zielpunktes
    > + * @return liefert Quadrat-Abstand zwischen den Map-Punkten als heuristische Funktion zur Entfernungsschaetzung;
    > + * je kleiner, desto mehr wird dieser Punkt bevorzugt behandelt
    > + */
    > +uint16 get_dist(uint16 xs, uint16 ys, uint16 xd, uint16 yd){
    > +	int16 xt=xd - xs;
    > +	int16 yt=yd - ys;
    > +
    > +	/* Abstandsermittlung nach dem guten alten Pythagoras ohne Ziehen der Wurzel */
    > +	return (xt*xt+yt*yt);
    > +
    > +}
    > +
    > +
    > +
    > +/*! ermittelt Abstossungs-Potenzial des Nodes xy, der bereits mit der Wahrscheinlichkeit eines
    > + *  Hindernisses laut Map belegt ist (-128 - +127 sind moegliche Map-Werte)
    > + *  das repulsive Potenzial ist am groessten umso naeher sich der Punkt am Hindernis befindet und
    > + *  liefert das kleinste Potenzial fuer am weitesten vom Hindernis weg
    > + * @param x x-Koord des zu untersuchenden Punktes
    > + * @param y y-Koord des zu untersuchenden Punktes
    > + * @param mapval Wert laut Map an Position xy
    > + * @return repulsives Potenzial fuer Map-Hinderniswahrscheinlichkeit
    > + */
    > +uint16 get_rep_pot(uint16 x, uint16 y, int8 mapval) {
    > +
    > +	// bereits markierter Pfadpunkt bekommt Hinderniswert, sonst wuerde Richtung des schon markierten
    > +	// Weges bevorzugt werden; so also nicht genommen
    > +    if (mapval==PATHNODES)
    > +	      mapval=-MAP_ART_HAZARD;
    > +
    > +	// alles groesser 0 ist Freiwahrscheinlichkeit, die beim Umherfahren fuer Freifelder eingetragen wird;
    > +	// daher auf 0 setzen mit Gleichbehandlung
    > +	if (mapval > 0)
    > +	     mapval = 0;
    > +
    > +    // kleinerer Hinderniswert bekommt hiermit groesseres Abstossungspotenzial und wird in den positiven Bereich
    > +    // transferiert
    > +    return mapval * (-1) + 127;
    > +}
    > +
    > +
    > +
    > +
    > +
    > +/*! ermittelt das gefaltete Potential an der Pos. xy bei kleiner Aufloesung; Belegungswahrscheinlichkeit haengt
    > + *  ab von einem moeglichen Hindernis vor oder nach dieser Map-Position, daher wird bei Hindernis drumrum das
    > + * Belegungspotenzial erhoeht wodurch Wahrscheinlichkeit der Verwendung sinkt
    > + * diagonale Felder erhalten hoeheren Wert, wodurch gerade Felder bevorzugt werden
    > + * nicht bei hoeherer Aufloesung: hier wird ueber Hindernisumkreis -muss gross genug sein ca. Radiusbreite - die
    > + * Hinderniswahrscheinlichkeit gesteuert
    > + * @param x x-Map-Koord des zu untersuchenden Punktes
    > + * @param y y-Map-Koord des zu untersuchenden Punktes
    > + * @param xd,yd xy-Koord des bereits decrementierten Nachbarpunktes
    > +*  @param xi,yi xy-Koord des bereits incrementierten Nachbarpunktes
    > + * @return gefaltetes Potenzial der direkten 8 Umgebungszellen
    > +*/
    > +
    > +
    > +#if MAP_RADIUS_FIELDS_GODEST <= 1  // nur kleine Aufloesung
    > +
    > +uint16 get_rep_folded_pot(uint16 x, uint16 y, uint16 xd, uint16 yd,uint16 xi, uint16 yi) {
    > +	int16 pot=0;
    > +	int8 tmp;
    > +
    > +	 if (map_in_dest(x,y,dest_x_map,dest_y_map))
    > +       return 0;  // Start- ist schon Zielpunkt - glechj beenden mit 0-Pot
    > +
    > +	// zuerst zeilenweise Faltung
    > +
    > +	if (xd >= map_min_x) {
    > +	  tmp=map_get_field(xd,y);
    > +	  if (tmp<0) //Hinderniswahrscheinlichkeit ist < 0
    > +	  	pot=get_rep_pot(xd,y,tmp) / 4;  //nur mit 1Viertel beruecksichtigen
    > +	}
    > +
    > +	  //gleiches an Stelle x+1
    > +	  if (xi <= map_max_x ) {
    > +	  tmp=map_get_field(xi,y);
    > +	  if (tmp<0) //Hinderniswahrscheinlichkeit ist < 0
    > +	  	//Abstossungspotential bei x-1 ermitteln
    > +	  	pot+=get_rep_pot(xi,y,tmp) / 4 ; //nur mit 1Viertel beruecksichtigen
    > +	}
    > +
    > +
    > +	//spaltenweise Faltung
    > +	if (yd > map_min_y) {
    > +	  tmp=map_get_field(x,yd);
    > +	  if (tmp<0) //Hinderniswahrscheinlichkeit ist < 0
    > +	  	//Abstossungspotential bei x-1 ermitteln
    > +	  	pot+=get_rep_pot(x,yd,tmp) / 4;  //nur mit 1Viertel beruecksichtigen
    > +	}
    > +
    > +	if (yi < map_max_y-1) {
    > +	  //gleiches an Stelle x+1
    > +	  tmp=map_get_field(x,yi);
    > +	  if (tmp<0) //Hinderniswahrscheinlichkeit ist < 0
    > +	  	//Abstossungspotential bei x-1 ermitteln
    > +	  	pot+=get_rep_pot(x,yi,tmp) / 4; //nur mit 1Viertel beruecksichtigen
    > +	}
    > +
    > +
    > +	//diagonale Felder ebenfalls beruecksichtigen
    > +	if (xd >= map_min_x  && yd > map_min_y) {
    > +	  tmp=map_get_field(xd,yd);
    > +	  if (tmp<0) //Hinderniswahrscheinlichkeit ist < 0
    > +	  	pot+=get_rep_pot(xd,yd,tmp) / 3; //nur mit 1Drittel beruecksichtigen
    > +	}
    > +
    > +	if (xd >= map_min_x && yi < map_max_y-1) {
    > +	  tmp=map_get_field(xd,yi);
    > +	  if (tmp<0) //Hinderniswahrscheinlichkeit ist < 0
    > +	  	pot+=get_rep_pot(xd,yi,tmp) / 3; //nur mit 1Drittel beruecksichtigen
    > +	}
    > +
    > +	if (xi <= map_max_x && yd > map_min_y) {
    > +	  tmp=map_get_field(xi,yd);
    > +	  if (tmp<0) //Hinderniswahrscheinlichkeit ist < 0
    > +	  	pot+=get_rep_pot(xi,yd,tmp) / 3; //nur mit 1Drittel beruecksichtigen
    > +	}
    > +
    > +	if (xi < map_max_x && yi < map_max_y-1) {
    > +	  tmp=map_get_field(xi,yi);
    > +	  if (tmp<0) //Hinderniswahrscheinlichkeit ist < 0
    > +	  	pot+=get_rep_pot(xi,yi,tmp) / 3; //nur mit 1Drittel beruecksichtigen
    > +	}
    > +
    > +	return pot;  // Summe aller Abstossungs-Faltungspotenziale zurueckgeben
    > +
    > +}
    > +
    > +#endif
    > +
    > +
    > +/*! liefert Wert fuer das attraktive Potenzial des Zieles aus Abstandsfunktion; bewirkt das
    > + *  Anziehen des Bots und Ablenken in Richtung Ziel; ist umso groesser je naeher der Punkt
    > + *  sich am Ziel befindet; wie ein Trichter liegt diese Kraft auf der Karte; am groessten
    > + *  ist die Kraft am Zielpunkt selbst, das Potenzial dort also am kleinsten; Potenzial ist
    > + *  entgegengesetzt der Kraft, wird also in Richtung Ziel immer kleiner
    > + * @param x x-Koord des zu untersuchenden Punktes
    > + * @param y y-Koord des zu untersuchenden Punktes
    > + * @param xdest x-Koord des Zieles
    > + * @param ydest y-Koord des Zieles
    > + * @return attraktives Potenzial aus Abstandsermittlung
    > + */
    > +uint16 get_att_pot(uint16 x, uint16 y, uint16 xdest, uint16 ydest) {
    > +  // Als heuristische Potenzialfunktion dient die Bestimmung des Abstandes zum Ziel, wobei der Punkt
    > +  // xy das kleinste Potential hat, welcher sich am nahesten am Ziel befindet und konvergiert dort gegen 0
    > +  // hoher Wert fuer hohen Abstand
    > + return (DIV_ATTPOT <= 1)  ? 4 * get_dist(x,y,xdest,ydest) : (4 * get_dist(x,y,xdest,ydest)) / DIV_ATTPOT;
    > +}
    > +
    > +/*! liefert verdoppeltes repulsives Potenzial, falls der zu untersuchende Punkt auf den Elternnode verweist
    > + *  somit Wahrscheinlichkeit hoeher, dass ein anderer Weg gegangen wird als wo er gerade herkommt
    > + *  @param xs ys  betrachtete Map-Kooordinaten
    > + *  @param reppot bereits ermitteltes Abstossungspotenzial von xs ys
    > + *  @return Verdopplung von reppot oder 0
    > + */
    > + uint16 pot_off_to_parent(uint16 xs, uint16 ys, uint16 reppot) {
    > +  return (xs==x_parent && ys==y_parent)  ? reppot * 2 : 0;
    > + }
    > +
    > +
    > +
    > +/*! liefert die Summe aller abstossenden Potenziale ohne anziehendes Zielpotenzial
    > + * @param xs ys betrachtete Koordinaten
    > + * @param xd yd Zielkoordinaten
    > + * @return Summe aller repulsiven Potenziale
    > + */
    > +uint16 get_all_rep_pot(uint16 xs, uint16 ys, uint16 xd, uint16 yd) {
    > +
    > +	uint16 sumpot=0;
    > +
    > +	//falls der zu untersuchende Knoten der Zielknoten ist, keine Abstossungskraft mit Pot 0
    > +	  if (!map_in_dest (xs,ys, xd,yd))
    > +	    sumpot=get_rep_pot(xs,ys,map_get_field(xs,ys));  //Gefahrenpot. auf Zelle selbst
    > +
    > +	sumpot+=pot_off_to_parent(xs,ys,sumpot);  // Potenzial verdoppelt falls Elternnode derjenige ist
    > +
    > +
    > +    // repulsives Faltungspotenzial bringt nur Sinn bei geringen Aufloesungen
    > +    #if MAP_RADIUS_FIELDS_GODEST <= 1
    > +	  sumpot+=get_rep_folded_pot(xs,ys,xs-1,ys-1,xs+1,ys+1);
    > +	#endif
    > +
    > +    return sumpot;  // Rueckgabe des repulsiven Summenpotenzials
    > +}
    > +
    > +
    > +
    > +/*!
    > + * Check, ob der Punkt xy mit destxy identisch ist (bei kleinen Aufloesungen) oder sich
    > + * innerhalb des halben Radius-Umkreises befindet (hoehere Aufloesungen); verwendet um das
    > + * Zielfahren mit gewisser Toleranz zu versehen
    > + * @param x x-Ordinate der Karte (nicht der Welt!!!)
    > + * @param y y-Ordinate der Karte (nicht der Welt!!!)
    > + * @param destx Ziel-x-Ordinate der Karte (nicht der Welt!!!)
    > + * @param desty Ziel-y-Ordinate der Karte (nicht der Welt!!!)
    > + * @return True wenn xy im Umkreis vorhanden oder identisch ist
    > + */
    > +int8 map_in_dest (uint16 x, uint16 y, uint16 destx, uint16 desty) {
    > +  // MAP_RADIUS_FIELDS sind die Mapfelder im Botdurchmesser
    > +  #if MAP_RADIUS_FIELDS_GODEST > 1 // gilt nur fuer hoehere Aufloesungen, sonst direkter Vergleich
    > +	  //Distanzen in Mapfelder
    > +	  int16 distx=destx-x;
    > +	  int16 disty=desty-y;
    > +	  // Radius, also Abstand ermitteln; Wurzel ziehen spare ich mir
    > +	  return distx*distx + disty*disty <= MAP_RADIUS_FIELDS_GODEST_HALF_QUAD;
    > +
    > +  #else
    > +    //bei geringer Aufloesung (MCU) direkt mit Zielkoords vergleichen
    > +  	return  (x==destx && y==desty);
    > +  #endif
    > +}
    > +
    > +
    > +
    > +
    > +
    > +
    > +/*! ab dem Knoten x,y wird rundum fuer jeden Knoten das Potenzial ermittelt und der Knoten mit dem kleinsten
    > + *  Wert bestimmt und in nextx y zurueckgegeben
    > + *  dies ist die Hauptroutine zur Ermittlung des Potenzials in einem beliebigen Punkt
    > + *  @param xdest, ydest Zielkoordinaten (zur Bestimmung des attraktiven Potenzials)
    > + *  @param XP_START, YP_START  Startkoordinaten des bots
    > + *  @param nextx nexty naechster Zwischenzielpunkt mit geringstem Potenzial
    > + *  @param get_next_pathpoint Bei True wird der bereits mit Kennung versehene Pfad zum Ziel verfolgt und der
    > + *    naechste bereits als Pfad gekennzeichnete Punkt zurueckgegeben; bei False wird Nachbar mit geringstem
    > + *    Potenzial ermittelt fuer Pathplaning
    > + *  @param roff Offsetradius zur Umkreissuche
    > + */
    > +void get_gradient(uint16 xdest, uint16 ydest,uint16 XP_START, uint16 YP_START,
    > +                  uint16 *nextx, uint16 *nexty, int8 get_next_pathpoint, uint8 roff) {
    > +  int8 i;
    > +  int8 j;
    > +  uint16 sumpot=0;
    > +
    > +  //minimalstes Potenzial wird sich hier gemerkt
    > +  uint16 minpot=9999;  //auf maximalen Wert setzen
    > +  uint16 x_minpos=0;
    > +  uint16 y_minpos=0;
    > +
    > +  // vorberechnete Spalten und Zeilenzaehler
    > +  uint16 xj=0;
    > +  uint16 yi=0;
    > +  uint16 sumpotrep=0;
    > +
    > +  uint16 x=XP_START;
    > +  uint16 y=YP_START;
    > +
    > +  // Rueckgabewerte erst mal initialisieren
    > +  *nextx=0;
    > +  *nexty=0;
    > +
    > +  int8 mapval=0;  // Wert des Mapfeldes
    > +
    > +  if (map_in_dest(dest_x_map,dest_y_map,XP_START,YP_START)) { // Start- ist schon Zielpunkt
    > +       *nextx=dest_x_map;
    > +       *nexty=dest_y_map;
    > +       return;
    > +  }
    > +
    > +  // Umkreisermittlung macht nur Sinn bei hoeherer Aufloesung
    > +  #if MAP_RADIUS_FIELDS_GODEST > 1
    > +    int8 radius = MAP_RADIUS_FIELDS_GODEST + roff;  // etwas weiter suchen
    > +    int16 rquad = radius * radius;
    > +    uint16 quad=0;                           // Quadratwert des Abstandes in der Schleife
    > +    uint16 roffq = (roff + 4) * (roff + 4);  // Wert ist Experimentwert um nicht zu viele Felder zu checken
    > +  #else
    > +    int8 radius = 1;  // sonst bei geringer Aufloesung direkter Nachbar
    > +  #endif
    > +
    > +
    > +     //nur innerhalb des Radiuskreises bleiben
    > +	 for (i=-radius; i<=radius; i++) {//Zeilenzaehler
    > +	 	    yi = y + i;
    > +
    > +			for (j=-radius; j<=radius; j++) {//Spaltenzaehler
    > +			  xj = x + j;
    > +
    > +			  #if MAP_RADIUS_FIELDS_GODEST > 1  // Umkreisfelder nur bei hoeherer Aufloesung, sonst 8Nachbarschaft
    > +			   if (radius>1)
    > +			     quad=i*i + j*j ;
    > +
    > +			     if ((!get_next_pathpoint && (quad == rquad || quad==rquad-1 || quad==rquad+1)) || // Potermittlung
    > +			         (get_next_pathpoint && quad >= MAP_RADIUS_FIELDS_GODEST_HALF_QUAD + roffq)) { // Umkreissuche des Weges
    > +
    > +              #endif
    > +
    > + 			    //Potenzial ermitteln an Indexposition; nicht Mittelpos 0,0 selbst
    > +			  	if (((j!=0)||(i!=0)) && xj>=0 && yi>=0) {
    > +
    > + 			     // nur innerhalb der Rechteckgrenzen
    > +			     if (xj>=map_min_x && xj<=map_max_x && yi>=map_min_y && yi<=map_max_y) {
    > + 			      //wenn nicht betretbar laut Map gar nicht erst auswerten und gleich uebergehen
    > +			      mapval=map_get_field(xj,yi);
    > +
    > +			      // entweder Pfad erst ermitteln oder den naechsten Punkt des Pfades zurueckgeben
    > +			      if (((xj!=x_parent||yi!=y_parent)&&(mapval>-MAPFIELD_IGNORE)&& !get_next_pathpoint)||
    > +			           (get_next_pathpoint && mapval==PATHNODES && (xj!=x_parent || yi!=y_parent))) {
    > +			      	 sumpotrep=9999;
    > +			      	 sumpot=9999;
    > +
    > +			      	// Pfadverfolgung; bereits gekennzeichnete Pfadpunkte werden gesucht im Botumkreis
    > +			      	// gibts mehrere, wird bei geringer Aufloesung derjenige mit kleinstem Abstand zum Ziel genommen
    > +	                 if (get_next_pathpoint && mapval==PATHNODES ) { // Punkt des Pfades gefunden auf dem Umkreis
    > +	                       sumpotrep=0;
    > +	                       //koennte auch mehrere geben, daher den am nahesten zum Ziel
    > +	                       sumpot=get_att_pot(xj,yi,xdest,ydest);
    > +
    > +	                      #if MAP_RADIUS_FIELDS_GODEST > 1	  	// nur bei hoeherer Aufloesung Schluss wg. Laufzeit
    > +	                        // Schluss wenn Punkt gefunden
    > +	                        *nextx=xj;
    > +			  	  	        *nexty=yi;
    > +			  	  	        return;
    > +			  	  	      #endif
    > +
    > +	                 }
    > +	                   // Potenzial wird neu berechnet, womit sich ein anderer Weg ergeben kann
    > +	                   else {
    > +	                     sumpotrep=get_all_rep_pot(xj,yi,xdest,ydest);
    > +			  	         sumpot=sumpotrep + get_att_pot(xj,yi,xdest,ydest);
    > +			                }
    > +
    > +
    > +			  	    //bei kleinerem Potenzial Werte merken,
    > +			  	    if (sumpot<minpot) {
    > +			  	  	  //Node darf nicht existieren um Nachbarn einzufuegen, bei Gradabstieg aber auch wenn noch nicht existiert
    > +			  	  	     minpot=sumpot;  // Potenzial nur mit Kosten zum Parent
    > +
    > +			  	  	     x_minpos=xj;
    > +			  	  	     y_minpos=yi;
    > +			  	  	     *nextx=xj;    // Rueckgabewerte setzen
    > +			  	  	     *nexty=yi;
    > +
    > +			  	    }//if sumpot<minpot
    > +
    > +			      }//if Mapfeld nicht betretbar
    > +			     } //ausserhalb der Indexgrenzen
    > +			  	}//if, Pos selbst
    > +             #if MAP_RADIUS_FIELDS_GODEST > 1	  	// nur bei hoeherer Aufloesung
    > +			   } //Suche war ab halben bis ganzem Botradius
    > +			 #endif
    > +			  } //for Spalten x->j
    > +	 } //for Zeilen y->i
    > +	// Botposition wird nun zur neuen Elternposition
    > +	x_parent=XP_START;
    > +	y_parent=YP_START;
    > +}
    > +
    > +
    > +
    > +
    > +// =================== endlich das eigentliche Fahrverhalten ================================
    > +
    > +
    > +/*! Verhalten, damit der bot der eingetragenen Map-Pfadlinie folgt; das Verhalten richtet den bot zuerst
    > + *  zum Endziel aus und faehrt in diese Richtung ohne Pfadplanung; erst wenn ein Hindernis voraus erkannt
    > + *  wird, erfolgt eine Pfadplanung wobei in der Map die Pfadpunkte in bestimmter Umkreisentfernung vermerkt
    > + *  werden; waehrend der Fahrt wird in der Map im Botumkreis der naechste Pfadpunkt gesucht und dieser
    > + *  angefahren und bei erreichen des Punktes wiederum der Folgepunkt gesucht; erst wenn keiner gefunden werden
    > + *  kann (durch Eintragen der Hinderniswahrscheinlichkeit) erfolgt eine neue Pfadplanung an Hand der Potenzialsuche
    > + *  @param *data der Verhaltensdatensatz
    > + */
    > +void bot_gotoxy_behaviour_map(Behaviour_t *data){
    > +	#define INITIAL_TURN 	0
    > +	#define DRIVE_TO_NEXT	2
    > +	#define POS_REACHED	    3
    > +	#define NEXT_POS	    4
    > +	#define INITIAL_GETPATH 5
    > +	#define GO_PATH 	    6
    > +	#define CLEAR_MAP 	    7
    > +	#define CLEAR_MAP_FREEFIELDS 8
    > +	#define DIRECTION_GOAL 	     9
    > +	#define NEXT_IS_HAZARD 	    10
    > +
    > +	static uint16 diffx_last=0;
    > +	static uint16 diffy_last=0;
    > +
    > +	static uint8 laststate = 0;
    > +
    > +	int16 X_pos;
    > +	int16 Y_pos;
    > +
    > +	int16 xDiff=0;
    > +	int16 yDiff=0;
    > +
    > +    // zur Erkennung, falls mehrmals dasselbe Ziel angesteuert wird, dies aber nicht geht
    > +    static int16 last_xhaz=0;
    > +    static int16 last_yhaz=0;
    > +
    > +   // war Hindernis voraus, wird dessen Mappositionen bestimmt
    > +	uint16 x_nexthaz=0;
    > +    uint16 y_nexthaz=0;
    > +
    > +	//Map-Mauspopsition selbst
    > +	X_pos=world_to_map(x_pos);
    > +	Y_pos=world_to_map(y_pos);
    > +
    > +		//Jederzeit Schluss wenn Positionen Maus und Endziel uebereinstimmen
    > +	 if (map_in_dest (X_pos, Y_pos,dest_x_map, dest_y_map)) {
    > +		gotoStatexy=POS_REACHED;
    > +		speedWishLeft=BOT_SPEED_STOP;
    > +		speedWishRight=BOT_SPEED_STOP;
    > +	}
    > +	else
    > +	 {	// Check ob naechster Punkt ueberhaupt befahrbar ist
    > +	 	if (map_get_field(next_x,next_y)<=-MAPFIELD_IGNORE && gotoStatexy!=DIRECTION_GOAL && gotoStatexy!=NEXT_IS_HAZARD)
    > +	         gotoStatexy=INITIAL_GETPATH;  // Neubestimmung notwendig
    > +	 }
    > +
    > +	// hier nun Aufruf der Funktionen nach den Verhaltenszustaenden
    > +	switch(gotoStatexy) {
    > +		case DIRECTION_GOAL:    // Ausrichten und Fahren zum Endziel, solange Weg frei
    > +		  next_x=dest_x_map;    // das naechste Ziel ist das Endziel
    > +		  next_y=dest_y_map;
    > +		  target_x = next_x;    // Zwischenziel identisch Endziel
    > +		  target_y = next_y;
    > +		  laststate = DIRECTION_GOAL;  // Kennung setzen dass ich aus Zielfahrt komme
    > +		  gotoStatexy=INITIAL_TURN;
    > +
    > +		   // falls sich Loch in der Naehe befindet, dann gleich ohne Zielfahrt mit Patenzialsuche weiter
    > +		   #if MAP_RADIUS_FIELDS_GODEST > 0
    > +		     if (value_in_circle(X_pos,Y_pos,-128)) {  // Loch im Umkreis Botradius bei hoeherer Aufloesung
    > +		   #else
    > +		    // bei geringer Aufloesung darf fuer Zielfahrt kein Loch in Nachbarschaft vorhanden sein
    > +			if (  // bei geringer MCU Aufloesung direkten Nachbarn abfragen
    > +			     map_get_field(X_pos-1,Y_pos)==-128 || map_get_field(X_pos+1,Y_pos)==-128 ||
    > +			     map_get_field(X_pos,Y_pos-1)==-128 || map_get_field(X_pos,Y_pos+1)==-128 ||
    > +			     map_get_field(X_pos-1,Y_pos-1)==-128 || map_get_field(X_pos-1,Y_pos+1)==-128 ||
    > +			     map_get_field(X_pos+1,Y_pos-1)==-128 || map_get_field(X_pos+1,Y_pos+1)==-128
    > +			) {
    > +		   #endif
    > +		  	laststate = CLEAR_MAP;  // nicht mehr direction_goal
    > +		  	gotoStatexy = CLEAR_MAP;
    > +		  }
    > +
    > +		  break;
    > +
    > +		case CLEAR_MAP:
    > +		   // MAP von altem Pfad und kuenstlichen Hindernissen befreien
    > +		   clear_map(True);
    > +		   gotoStatexy=CLEAR_MAP_FREEFIELDS;
    > +		   break;
    > +
    > +		case CLEAR_MAP_FREEFIELDS:
    > +		   // MAP nun von Freifeldern befreien-Originalmap wieder da ohne Freiwahrscheinlichkeiten voraus
    > +		   clear_map(False);
    > +		   gotoStatexy=INITIAL_GETPATH;
    > +
    > +		   break;
    > +
    > +		case INITIAL_GETPATH:
    > +		    // naechsten Punkt nach Potenzialfeldmethode ermitteln, Wert steht dann in next_x, next_y
    > +		    next_x=0;
    > +		    next_y=0;
    > +		    bot_path_bestfirst(data);   //Umschaltung Pfadsuche mit Deaktivierung dieses Verhaltens
    > +		    laststate=INITIAL_GETPATH;
    > +		    gotoStatexy=GO_PATH;
    > +
    > +			break;
    > +
    > +		case GO_PATH:
    > +		    // kommt hierher nach Beendigung des Pfadplanungsverhaltens
    > +		    // Pfadplanung muss Ergebnis liefern, sonst gehts eben nicht weiter
    > +
    > +			// Pfadplanung hat naechsten anzufahrenden Punk um den Bot ermittelt
    > +            if (next_x==0 && next_y==0) {  // nichts gefunden -> Ende
    > +				 gotoStatexy=POS_REACHED;  // Kein Ziel mehr gefunden go_path und Schluss
    > +				 break;
    > +			}
    > +
    > +            // Zwischenziel belegen
    > +		    target_x = next_x;
    > +		    target_y = next_y;
    > +
    > +			gotoStatexy=INITIAL_TURN;      // als naechstes Drehen zum Zwischenziel
    > +
    > +			break;
    > +
    > +		case INITIAL_TURN:
    > +		    // hier erfolgt Ausrichtung zum naechsten Zwischenzielpunkt durch Aufruf des bereits
    > +		    // vorhandenen bot_turn-Verhaltens
    > +		    xDiff = target_x-X_pos;
    > +		    yDiff = target_y-Y_pos;
    > +		    diffx_last=fabs(xDiff);
    > +		    diffy_last=fabs(yDiff);
    > +			gotoStatexy=DRIVE_TO_NEXT;      // gedreht sind wir hiernach zum Zwischenziel, also fahren
    > +
    > +            // Umschaltung zum Drehverhalten; Drehung zum Zwischenziel wenn noetig
    > +			bot_turn(data,bot_gotoxy_calc_turn(xDiff,yDiff));
    > +
    > +			// Aktivierung des Verhaltens der Haengenbleiben-Erkennung; laeuft parallel zu diesem
    > +			// Fahr-Verhalten (falls es noch nicht laeuft); beim Haengenbleiben wird eine Sync-Variable
    > +			// gesetzt (_fired), die hier ausgewertet und rueckgesetzt werden muss
    > +	        bot_check_hang_on(0);
    > +
    > +			break;
    > +
    > +
    > +		case DRIVE_TO_NEXT:
    > +		    // hier wird zum Zwischenziel gefahren mit Auswertung auf Haengenbleiben und Locherkennung
    > +		    xDiff=target_x-X_pos;
    > +	        yDiff=target_y-Y_pos;
    > +
    > +			// Position erreicht?
    > +			 if (map_in_dest (X_pos, Y_pos,target_x, target_y)) {
    > +
    > +				//Zwischenziel erreicht, ist dies auch Endziel dann Schluss
    > +				 if (map_in_dest (X_pos, Y_pos,dest_x_map, dest_y_map)) {
    > +				    gotoStatexy=POS_REACHED;  // Endziel erreicht und Schluss
    > +				    break;
    > +				}
    > +				else {
    > +			 	      // Zwischenziel wurde erreicht; bei hoeherer Aufloesung ist der bot nur in der Naehe
    > +					  gotoStatexy=NEXT_POS;  // zum Ermitteln der naechsten Position
    > +
    > +					  // ist der Bot auf gewollte Zwischenzielpos, dann die Pfadkennnung
    > +					  // wegnehmen, sonst kann es zu Endlosschleife im lok. Min. kommen
    > +					  if (map_get_field(X_pos,Y_pos)==PATHNODES)
    > +						   map_set_field(X_pos,Y_pos,0);
    > +
    > +					// vorsichtige Weiterfahrt
    > +					  speedWishLeft  = BOT_SPEED_FOLLOW;
    > +					  speedWishRight = BOT_SPEED_FOLLOW;
    > +					  break;
    > +				}
    > +			}  //Ende Endzielauswertung
    > +
    > +			// Check auf zu nah oder nicht befahrbar
    > +			if  (sensDistL<=OPTIMAL_DISTANCE || sensDistR<=OPTIMAL_DISTANCE ||   // < 144mm
    > +			     map_get_field(target_x,target_y) <= -MAPFIELD_IGNORE ) {        // Mapposition nicht befahrbar
    > +
    > +			     if (laststate==DIRECTION_GOAL) {   // Abbruch der Zielfahrt und Pfadsuche
    > +			      gotoStatexy=INITIAL_GETPATH;
    > +			      break;
    > +			     }
    > +			     else {
    > +			       if (map_in_dest(target_x,target_y,dest_x_map,dest_y_map)) {  // Schluss bei Endziel
    > +                     gotoStatexy=POS_REACHED;
    > +                     break;
    > +			       }
    > +			       else {
    > +                     gotoStatexy=NEXT_IS_HAZARD;  // Ziel noch nicht erreicht, naechstes Ziel aber Hindernis
    > +                     break;
    > +			       }
    > +			     }
    > + 			}
    > +
    > +
    > +			// wenn Map-Abgrundverhalten Loch erkennt, da ja bot gerade ueber Abgrund gehangen hatte, ist
    > +			// neue Wegsuche mit Hindernis voraus aufzuruefen
    > +			#ifdef BEHAVIOUR_BORDER_MAP_AVAILABLE
    > +			 if  (border_behaviour_fired) {   // Syncvar wurde vom Abgrundverhalten gesetzt; hat also gefeuert
    > +               border_behaviour_fired=False;  // wieder ruecksetzen, erst durch Abgrundverhalten neu gesetzt
    > +			   gotoStatexy=NEXT_IS_HAZARD;	  // Hindernis voraus
    > +			   break;
    > +             }
    > +            #endif
    > +
    > +			//Abstand wird wieder groesser; neue Pfadsuche
    > +			if (laststate!=DIRECTION_GOAL && ( diffx_last<fabs(xDiff)|| diffy_last<fabs(yDiff))) {
    > +				gotoStatexy=NEXT_POS;
    > +				speedWishLeft  = BOT_SPEED_FOLLOW;  // vorsichtige Weiterfahrt
    > +				speedWishRight = BOT_SPEED_FOLLOW;
    > +				break;
    > +			}
    > +
    > +
    > +           // hier Check ob Haengenbleiben-Verhalten zugeschlagen hat
    > +		   if (hangon_behaviour_fired) {    // Syncvar wurde vom H.-Verhalten gesetzt
    > +		   	 gotoStatexy=NEXT_IS_HAZARD;    // Hindernis voraus
    > +		   	 hangon_behaviour_fired=False;  // Syncvar wieder ruecksetzen
    > +		     break;
    > +		   }
    > +
    > +
    > +		    // hier ist der eigentliche Antrieb fuer das Fahren
    > +			#ifdef PC
    > +			   speedWishLeft=BOT_SPEED_FAST;
    > +			   speedWishRight=BOT_SPEED_FAST;
    > +
    > +             #else
    > +			   speedWishLeft=BOT_SPEED_MEDIUM;
    > +			   speedWishRight=BOT_SPEED_MEDIUM;
    > +			 #endif
    > +
    > +
    > +            // Abstandsdifferenz aus diesem Durchlauf merken
    > +			diffx_last=fabs(xDiff);
    > +		    diffy_last=fabs(yDiff);
    > +			break;
    > +
    > +       case NEXT_IS_HAZARD:
    > +           // das naechste Ziel kann nicht erreicht werden und wird hinderniswahrscheinlicher
    > +           // Bot ist hier schon rueckwaerts gefahren, steht also nicht mehr auf Abgrundfeld
    > +           // wenn mehrmals hintereinander dasselbe Ziel ermittelt wird aber Hindernis darstellt,
    > +           // dann auch auf Hind. setzen
    > +           if   (last_xhaz==target_x && last_yhaz==target_y) {     // gleiches Ziel wie vorher erkannt
    > +            	 	 map_set_value_occupied(target_x,target_y,-MAP_ART_HAZARD);  // hinderniswahrscheinlicher
    > +           }
    > +           else {
    > +             last_xhaz=target_x;  // Hindernisziel aus diesem Durchlauf merken
    > +             last_yhaz=target_y;
    > +           }
    > +
    > +           // 10 cm voraus Hindernisposition laut Map bestimmen
    > +	       get_mappos_dist(x_pos,y_pos,(heading * M_PI /180),100,&x_nexthaz,&y_nexthaz);
    > +
    > +	       //Abstand erhoehen auf 15cm falls Botpos ermittelt wurde
    > +	       if (x_nexthaz==X_pos && y_nexthaz==Y_pos)
    > +	           get_mappos_dist(x_pos,y_pos,(heading * M_PI /180),150,&x_nexthaz,&y_nexthaz);
    > +
    > +		   // neue Pfadplanung wird erforderlich
    > +		   gotoStatexy=INITIAL_GETPATH;
    > +
    > +		   // Hohe Hinderniswahrscheinlichkeit mit Umfeldaktualisierung der -50 in Map eintragen
    > +		   map_set_value_occupied(x_nexthaz,y_nexthaz,-50);
    > +
    > +  	       // ist naechstes Ziel nicht Endziel, dann mehrmals Richtung Hindernis mit Umfeld druecken
    > +  	       if (target_x != dest_x_map && target_y != dest_y_map)
    > +			       map_set_value_occupied(target_x,target_y,-50);
    > +
    > +		   break;
    > +
    > +
    > +
    > +        case NEXT_POS:
    > +                    // das naechste Zwischenziel wurde erreicht (geringe Aufloesung) oder bot befindet sich in gewissem
    > +                    // Umkreis (hohe Aufloesung); hier ist der naechste in der Map vermerkte Pfadpunkt zu suchen oder die
    > +                    // Pfadsuche neu anzustossen
    > +				    // wichtig bei hoeherer Aufloesung: Der eigentliche Zielpunkt wird konkret kaum genau erreicht, daher
    > +				    // diesen vom Pfad nehmen und im Umkreis suchen; Abstossungspot setzen damit Benachteiligung bei Neurechnung
    > +				    if (map_get_field(target_x,target_y)==PATHNODES)
    > +				    	  map_set_value_occupied (target_x,target_y, -50);
    > +
    > +                      // Suche des naechsten Pfadpunktes in der Map zur Pfadverfolgung
    > +                      #if MAP_RADIUS_FIELDS_GODEST > 1
    > +                         // bei hoher Aufloesung mit Radiusumkreis entsprechend Aufloesungsfelder
    > +                       	 get_gradient(dest_x_map, dest_y_map, X_pos,Y_pos,&next_x,&next_y,True, MAP_RADIUS_FIELDS_GODEST);
    > +                      #else
    > +                         // bei geringer MCU Aufloesung immer das naechste Mapfeld
    > +                         get_gradient(dest_x_map, dest_y_map, X_pos,Y_pos,&next_x,&next_y,True,0);
    > +                       #endif
    > +
    > +                      // bei Abgrund drumrum nicht naechsten Punkt nehmen sondern Map-Potneubestimmung
    > +
    > +			         // bei Abgrund neben bot ist neue Pfadsuche erforderlich
    > +			         #if MAP_RADIUS_FIELDS_GODEST > 0
    > +			           if (value_in_circle(X_pos,Y_pos,-128)) {  // bei hoeherer Aufloesung mit Umkreissuche
    > +			         #else
    > +			          if (  // bei geringer MCU Aufloesung direkten Nachbarn abfragen
    > +			              map_get_field(X_pos-1,Y_pos)==-128 || map_get_field(X_pos+1,Y_pos)==-128 ||
    > +			              map_get_field(X_pos,Y_pos-1)==-128 || map_get_field(X_pos,Y_pos+1)==-128 ||
    > +			              map_get_field(X_pos-1,Y_pos-1)==-128 || map_get_field(X_pos-1,Y_pos+1)==-128 ||
    > +			              map_get_field(X_pos+1,Y_pos-1)==-128 || map_get_field(X_pos+1,Y_pos+1)==-128
    > +			          ) {
    > +
    > +			         #endif
    > +			          	 // naechster Pfadpunkt ist ungueltig und macht Neuermittlung notwendig
    > +			          	 next_x=0;
    > +			          	 next_y=0;
    > +
    > +                       }
    > +
    > +                    if (next_x==0 && next_y==0) {  // neue Pfadsuche erforderlich mit Maploeschung
    > +                    	gotoStatexy=CLEAR_MAP;
    > +                    	break;
    > +                    }
    > +
    > +                    // naechstes Zwischenziel ist der neu ermittelte Pfadpunkt laut Map
    > +                    target_x = next_x;
    > +		            target_y = next_y;
    > +
    > +
    > +		            // weiter zum Ausrichten auf neuen Pfadpunkt mit Drehung
    > +		            gotoStatexy=INITIAL_TURN;
    > +
    > +		            // langsam weiterfahren
    > +		             #ifdef PC
    > +			            speedWishLeft=BOT_SPEED_NORMAL;
    > +			            speedWishRight=BOT_SPEED_NORMAL;
    > +
    > +                     #else
    > +			            speedWishLeft=BOT_SPEED_MEDIUM;
    > +			            speedWishRight=BOT_SPEED_MEDIUM;
    > +			         #endif
    > +
    > +             break;
    > +
    > +
    > +		case POS_REACHED:
    > +		    // hier ist das gewuenschte Endziel erreicht
    > +			gotoStatexy=DIRECTION_GOAL;
    > +			// Parallelverhalten ausschalten da nicht mehr benoetigt
    > +			deactivateBehaviour(bot_check_hang_on_behaviour);
    > +			return_from_behaviour(data);
    > +			break;
    > +	}
    > +
    > +}
    > +
    > +
    > +
    > +
    > +/*! Bot faehrt auf eine bestimmt Map-Position relativ zu den akt. Botkoordinaten; globales Ziel
    > + *  wird mit den neuen Koordinaten belegt bei Uebergabe xy ungleich 0
    > + * @param *caller der Verhaltensdatensatz
    > + * @param x zu der x-Botkoordinate relative anzufahrende Map-Position
    > + * @param y zu der y-Botkoordinate relative anzufahrende Map-Position
    > + */
    > +void bot_gotoxy_map(Behaviour_t *caller, int16 x, int16 y){
    > +	//Mauskoords in  Mapkoordinaten
    > +	int16 X_pos = world_to_map(x_pos);
    > +	int16 Y_pos = world_to_map(y_pos);
    > +
    > +	//globale Zielkoordinaten setzen, die fuer Pfadplaner Verwendung finden
    > +	if (x!=0 || y!=0) {
    > +	  dest_x_map=X_pos + x;
    > +	  dest_y_map=Y_pos + y;
    > +	}
    > +
    > +    /* Kollisions-Verhalten ausschalten bei hoeherer Aufloesung; hier kann mit groesserem
    > +     * Hindernis-Umkreis ein groesserer Abstand erzielt werden
    > +	 */
    > +	 #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
    > +		 #if MAP_RADIUS_FIELDS_GODEST > 5  // nicht bei bot-Aufloesung, nur bei hoeherer Aufloesung
    > +				deactivateBehaviour(bot_avoid_col_behaviour);
    > +	     #endif
    > +	 #endif
    > +
    > +     #ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE
    > +        // das bisher vorhandene Abgrund-Notverhalten ist zu deaktivieren falls vorhanden
    > +        deactivateBehaviour(bot_avoid_border_behaviour);
    > +     #endif
    > +
    > +   /* Einschalten sicherheitshalber des on_the_fly Verhaltens, wird bei Notaus sonst deaktiviert
    > +    */
    > +    #ifdef BEHAVIOUR_SCAN_AVAILABLE
    > +		activateBehaviour(bot_scan_onthefly_behaviour);
    > +	#endif
    > +
    > +	// Verhalten zum Setzen der Abgrunderkennung sicherheitshalber ein
    > +	#ifdef BEHAVIOUR_BORDER_MAP_AVAILABLE
    > +	  activateBehaviour(bot_set_border_in_map_behaviour);
    > +	#endif
    > +
    > +
    > +    gotoStatexy=DIRECTION_GOAL;  // auf Zielfahrt init.
    > +   	switch_to_behaviour(caller, bot_gotoxy_behaviour_map, OVERRIDE);  // Umschaltung zum Fahr-Verhalten
    > +}
    > +
    > +
    > +/*! Verhalten laesst den Bot zum Endziel fahren, welches vorher auf eine bestimmte Botposition festgelegt wurde;
    > + *  ansonsten ist das Ziel der Bot-Startpunkt
    > + *  @param *caller der Verhaltensdatensatz
    > + */
    > +void bot_gotodest_map(Behaviour_t *caller){
    > +	// zum bereits gesetztem Endziel gehen mit 0-Differenz
    > +	bot_gotoxy_map(caller, 0,0);
    > +}
    > +
    > +
    > +/*! Verhalten zur Generierung des Pfades vom Bot-Ausgangspunkt zum Ziel nach der Potenzialfeldmethode;
    > + *  es wird der Weg ermittelt von der Botposition mit hohem Potenzial bis zum gewuenschten Ziel mit geringem
    > + *  Potenzial; Um den Hindernissen herum existiert laut den Mapwerten ein abstossendes Potenzial und die
    > + *  Entfernung zum Ziel dient als heuristisches Mittel fuer das global anziehende Potenzial; die Wegpunkte werden
    > + *  mit bestimmter Kennung in die Map eingetragen und diese Pfadpunkte im Fahrverhalten gesucht und nacheinander
    > + *  angefahren
    > + *
    > + * @param *data der Verhaltensdatensatz
    > + */
    > +void bot_path_bestfirst_behaviour(Behaviour_t *data){
    > +	#define GET_PATH_INITIAL  0
    > +	#define GET_PATH_BACKWARD 1
    > +	#define REACHED_POS       9
    > +
    > +	static uint8 state=GET_PATH_INITIAL;
    > +	static uint8 pathcounter=0;  // Abbruchzaehler fuer Pfadermittlung
    > +
    > +    // ausgehend von diesem Punkt wird das Potenzial ermittelt
    > +    static uint16 X=0;
    > +    static uint16 Y=0;
    > +    static uint16 x_first=0;       // 1. anzufahrender Punkt laut Pfadplanung ab Botposition
    > +    static uint16 y_first=0;
    > +
    > +	switch (state){
    > +		case GET_PATH_INITIAL:
    > +
    > +			// alle als frei markierten Mappositionen > 0 wieder auf 0 setzen
    > +	       clear_map(False);  // Loeschen der Freifelder, nicht der kuenstlichen Hindernisse
    > +
    > +
    > +		  // Bot-Map-Position ermitteln
    > +		  X=world_to_map(x_pos);
    > +		  Y=world_to_map(y_pos);
    > +
    > +		  // Eltern init auf Botpos.
    > +		  x_parent=X;
    > +          y_parent=Y;
    > +
    > +          // erste anzufahrende Botposition init.
    > +          x_first=0;
    > +          y_first=0;
    > +
    > +		  //  Pfadermittlung kann nun losgehen
    > +		  state=GET_PATH_BACKWARD;
    > +		  break;
    > +
    > +		case GET_PATH_BACKWARD:
    > +             // Gradientenabstieg-naechste Koord mit kleinstem Potenzial ab Punkt X,Y ermitteln
    > +			get_gradient(dest_x_map, dest_y_map, X,Y,&next_x,&next_y,False,0);
    > +
    > +
    > +            // Schluss falls kein Wert ermittelbar
    > +            if (next_x==0 && next_y==0) {
    > +             	state = REACHED_POS;
    > +            	break;
    > +            }
    > +
    > +            // am Endziel mit Pfadplanung angekommen und Schluss
    > +            if (map_in_dest(next_x,next_y,dest_x_map,dest_y_map)) {
    > +              map_set_field(dest_x_map,dest_y_map,PATHNODES);  // Endziel auch mit Pfadpunkt markieren
    > +              state = REACHED_POS;
    > +              if (x_first==0) {
    > +              	 x_first=next_x;
    > +                 y_first=next_y;
    > +              }
    > +              break;
    > +            }
    > +
    > +            // lokales Minimum -Sackgasse- wird erkannt; versucht hier rauszukommen durch Auffuellen
    > +            if ((next_x==X && next_y==Y) || value_in_circle(next_x,next_y,PATHNODES)	) {
    > +            	// hier wird der bereits beschrittene Weg erreicht, kein anderer Zielpunkt hat kleineres Pot
    > +            	// Pot dieses Punktes nun kuenstlich vergrossern Richtung Hindernis und neu suchen
    > +
    > +                // im halben Botradius auf kuenstl. Hindernis setzen
    > +                map_set_value_occupied(next_x,next_y,-MAP_ART_HAZARD); // ausgeschlossen von Pfadsuche
    > +
    > +                // evtl. auch Vaterfeld mit Hinderniswert belegen
    > +                if (x_parent!=0 && y_parent!=0)
    > +                  map_set_value_occupied(x_parent,y_parent,-50);       // benachteiligt beruecksichtigt
    > +
    > +                state=GET_PATH_INITIAL;                                // neue Pfadsuche
    > +                pathcounter++;                                         // Pfadsuchzaehler erhoeht
    > +
    > +                if (pathcounter>MAXCOUNTER)                            // Schluss wenn Maximum erreicht
    > +                 	state = REACHED_POS;
    > +
    > +                next_x=0;                                              // nix gefunden
    > +                next_y=0;
    > +
    > +            	break;
    > +            }
    > +            else {
    > +               // ersten Punkt um botpos merken, da dieser sonst ueberschrieben werden kann durch
    > +               // Mapaktualisierung der Botpos
    > +               if (x_first==0) {
    > +            	    x_first=next_x;
    > +            	    y_first=next_y;
    > +                 }
    > +              }
    > +
    > +
    > +            // Abfrage ob Bot-Zielposition erreicht wurde und damit Pfad erfolgreich bestimmt
    > +            if (state!=REACHED_POS && map_in_dest (world_to_map(x_pos),world_to_map(y_pos), dest_x_map,dest_y_map))
    > +                state = REACHED_POS;
    > +
    > +            // naechsten Zielpunkt in die Map eintragen
    > +            map_set_field(next_x,next_y,PATHNODES);
    > +
    > +            x_parent=X;  // als Parent den aktuellen Punkt merken
    > +            y_parent=Y;
    > +
    > +            X=next_x;    // der weiter zu untersuchende Punkt ist der Punkt aus Gradientenabstieg
    > +            Y=next_y;
    > +
    > +			break;
    > +
    > +
    > +		default:
    > +			state=GET_PATH_INITIAL;
    > +            next_x=x_first; // 1. Punkt des Weges ab Botposition zurueckgeben
    > +            next_y=y_first;
    > +            pathcounter=0;
    > +			return_from_behaviour(data);
    > +			break;
    > +	}
    > +}
    > +
    > +/*! Botenverhalten zum Aufruf des Pfadplanungs-Verhaltens
    > + * @param *caller der Verhaltensdatensatz
    > + */
    > +void bot_path_bestfirst(Behaviour_t *caller) {
    > +
    > +	// Botposition in Map-Koordinaten berechnen
    > +	uint16 X = world_to_map(x_pos);
    > +	uint16 Y = world_to_map(y_pos);
    > +
    > +	if (map_in_dest (X,Y, dest_x_map,dest_y_map)) {
    > +      #ifdef PC
    > +        LOG_INFO(("Botposition ist bereits das Ziel: %2d %2d \n",dest_x_map,dest_y_map));
    > +      #endif
    > +	}
    > +	else  {
    > +	  // Ziel bekommt freieste Feldwahrscheinlichkeit
    > +	  map_set_field(dest_x_map,dest_y_map,GOAL);
    > +
    > +	  // Botpos selbst erhaelt hohes Abstossungspotenzial
    > +	  map_set_field(X,Y,-90);
    > +
    > +	  // Umschaltung zur Pfadplanung
    > +	  switch_to_behaviour(caller, bot_path_bestfirst_behaviour,OVERRIDE);
    > +	}
    > +}
    > +
    > +/*! Auf dem Display wird der Mapausschnitt um die Botposition dargestellt, man sieht daran, wie der
    > + * bot Hindernisse (1) in der Umgebung erkannt hat und welches das naechste Ziel ist
    > + */
    > +void show_labmapmcu(void) {
    > +  char a[11];
    > +  int8 tmp;
    > +  int8 ind=0;
    > +  int8 rowcount=2;
    > +  char tmpr;
    > +  uint16 x_posw=world_to_map(x_pos);
    > +  uint16 y_posw=world_to_map(y_pos);
    > +  int16 x;
    > +  int16 y;
    > +
    > +  x=x_posw;
    > +  y=y_posw;
    > +
    > +   for (y=-1; y<= 1; y++) {
    > +   	ind=0;
    > +    for (x=5; x>= -5; x--) {
    > +
    > +    tmp=map_get_field(x_posw+x,y_posw+y);
    > +    //printf("y: %1d, pos xy: %1d %1d, wert %1d\n", y,x_posw+x,y_posw+y,tmp);
    > +    tmpr = '0';//0
    > +				if (tmp <= -MAPFIELD_IGNORE && tmp>-128 )
    > +				  tmpr = '1';//1
    > +				if (tmp == -128)
    > +				  tmpr = '7'; //7
    > +				if (tmp == PATHNODES)
    > +				    tmpr = '9'; //5
    > +
    > +				if (x_posw+x==x_posw && y_posw+y==y_posw)
    > +				    tmpr='3';
    > +
    > +				if (x+x_posw==dest_x_map && y_posw+y==dest_y_map)
    > +				    tmpr='8';
    > +
    > +    a[ind] = tmpr;
    > +    ind++;
    > +    }
    > +      display_cursor(rowcount,1);
    > +      display_printf(a);
    > +      rowcount++;
    > +   }
    > +
    > +}
    > +
    > +#ifdef PC
    > +/*! Hilfsroutine zur Kartenausgabe auf der Konsole; nur sinnvolle Ausgabe bei geringer Aufloesung
    > + */
    > +void show_labmap(void){
    > +
    > +		uint16 x,y;
    > +
    > +		//get_rectcoords_environment(&map_xmin,&map_xmax,&map_ymin,&map_ymax);
    > +        uint16 x_posw=0;
    > +	    uint16 y_posw=0;
    > +
    > +
    > +	    x_posw=world_to_map(x_pos);
    > +	    y_posw=world_to_map(y_pos);
    > +
    > +		int8 tmp;
    > +		int8 tmpr;
    > +
    > +		//for (y=map_max_y-1; y>= map_min_y; y--) {
    > +		 for (y=map_min_y; y<= map_max_y; y++) {
    > +		   printf("xy: %2d %2d :", map_min_x+1,y);
    > +
    > +			  for (x=map_max_x; x> map_min_x; x--) {
    > +
    > +				tmp=map_get_field(x,y);
    > +
    > +				tmpr = SPACE;//0
    > +				if (tmp <= -MAPFIELD_IGNORE && tmp>-128 )
    > +				  tmpr = WALL;//1
    > +				if (tmp == -128)
    > +				  tmpr = HOLE; //7
    > +				if (tmp == PATHNODES)
    > +				    tmpr = 9; //5
    > +
    > +
    > +				if (x_posw==x && y_posw==y)
    > +				    tmpr=BOTPOS;
    > +
    > +				if (x==dest_x_map && y==dest_y_map)
    > +				    tmpr=8;  // ziel sichtbar machen
    > +
    > +				printf("%1d|",tmpr);
    > +			}
    > +		    printf("\n");
    > +
    > +		}
    > +
    > +		//noch einmal mit konkreten Werten
    > +		 for (y=map_min_y; y<= map_max_y; y++) {
    > +		    printf("xy:%2d %2d : ",map_min_x+1,y);
    > +			    for (x=map_max_x; x> map_min_x; x--) {
    > +			    	if (x < 0) {
    > +		         printf("x 1 war kleiner= 0: %1d %1d", x,y);
    > +		         return;
    > +		         }
    > +				tmp=map_get_field(x,y);
    > +
    > +				printf("%3d|",tmp);
    > +			}
    > +		    printf("\n");
    > +		}
    > +
    > +      show_labmapmcu();  // Displayausgabe
    > +	}
    > +
    > +#endif
    > +
    > +
    > +#ifdef DISPLAY_AVAILABLE
    > +	#ifdef DISPLAY_MAP_GO_DESTINATION
    > +
    > +	static void mapgo_disp_key_handler(void){
    > +		/* Keyhandling um Map-Verhalten zu starten*/
    > +		switch (RC5_Code){
    > +			case RC5_CODE_3:
    > +			       // Setzen des Endziels auf die Botposition
    > +			       display_cursor(4,1);
    > +			       display_printf("F3");
    > +			       RC5_Code = 0;
    > +			       bot_set_destination(0);
    > +			       break;
    > +			 case RC5_CODE_4:
    > +			       // das eigentliche Fahrverhalten
    > +			       display_cursor(4,1);
    > +			       display_printf("F4");
    > +			       RC5_Code = 0;
    > +			       clear_map(True); // Loeschen der kuenstlichen Hindernisse und des alten Pfades fuer Neubeginn
    > +			       next_x=0;
    > +			       next_y=0;
    > +			       bot_gotodest_map(0);
    > +			      break;
    > +			  case RC5_CODE_6:
    > +			       // Ausgabe der Map auf der Konsole fuer PC oder nur Display fuer MCU
    > +			       RC5_Code = 0;
    > +			       #ifdef PC
    > +			         show_labmap();
    > +			       #else
    > +			         show_labmapmcu();  // Displayausgabe
    > +			       #endif
    > +			       break;
    > +		     }//case
    > +
    > +	}  // Ende Keyhandler
    > +
    > +
    > +		/*!
    > +		 * @brief	Display zum Start der Map-Routinen
    > +		 */
    > +		void mapgo_display(void){
    > +
    > +		  display_cursor(1,1);
    > +		  display_printf("MAP-GO-DEST");
    > +		  mapgo_disp_key_handler();		  // registrieren des Key-Handlers
    > +
    > +		}
    > +	#endif	// MAPGO_DISPLAY
    > +#endif
    > +
    > +#endif  // BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    > Index: include/bot-logic/behaviour_map_go_destination.h
    > ===================================================================
    > RCS file: include/bot-logic/behaviour_map_go_destination.h
    > diff -N include/bot-logic/behaviour_map_go_destination.h
    > --- /dev/null	1 Jan 1970 00:00:00 -0000
    > +++ include/bot-logic/behaviour_map_go_destination.h	1 Jan 1970 00:00:00 -0000
    > @@ -0,0 +1,77 @@
    > +/*
    > + * c't-Bot
    > + *
    > + * This program is free software; you can redistribute it
    > + * and/or modify it under the terms of the GNU General
    > + * Public License as published by the Free Software
    > + * Foundation; either version 2 of the License, or (at your
    > + * option) any later version.
    > + * This program is distributed in the hope that it will be
    > + * useful, but WITHOUT ANY WARRANTY; without even the implied
    > + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
    > + * PURPOSE. See the GNU General Public License for more details.
    > + * You should have received a copy of the GNU General Public
    > + * License along with this program; if not, write to the Free
    > + * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
    > + * MA 02111-1307, USA.
    > + *
    > + */
    > +
    > +/*! @file 	behaviour_map_go_destination.h
    > + * @brief 	Verhalten
    > + *          -zum Setzen eines Ziels mittel RC-Taste
    > + *          -um zu diesem Ziel zu gehen von einer beliebigen MAP-Position aus unter Umgehung
    > + *           aller zu diesem Zeitpunkt bekannten Hindernisse; Pfadplanung erfolgt mittels globaler
    > + *           Potenzialfeldmethode und der berechneten Potenziale laut des erstellten MAP-Grids mit den
    > + *           Hindernis-Wahrscheinlichkeiten
    > + *
    > + * @author 	Frank Menzel (Menzelfr@xxxxxxx)
    > + * @date 	30.04.07
    > +*/
    > +
    > +#include "bot-logic/bot-logik.h"
    > +
    > +#ifndef BEHAVIOUR_MAP_GO_DESTINATION_H_
    > +#define BEHAVIOUR_MAP_GO_DESTINATION_H_
    > +
    > +
    > +#ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    > +
    > +
    > +//globale Zielkoordinaten
    > +extern volatile uint16 dest_x_map;
    > +extern volatile uint16 dest_y_map;
    > +
    > +
    > +/*! Verhalten zur Pfadplanung */
    > +void bot_path_bestfirst(Behaviour_t *data);
    > +void bot_path_bestfirst_behaviour(Behaviour_t *data);
    > +
    > +
    > +/*! Verhalten zum Setzen der Zielkoordinaten */
    > +void bot_set_destination(Behaviour_t *data);
    > +void bot_set_destination_behaviour(Behaviour_t *data);
    > +
    > +/*! Verhalten zur Abgrunderkennung und eintragen in die Map */
    > +void bot_set_border_in_map_behaviour(Behaviour_t *data);
    > +
    > +/*! Verhalten zum Erkennen, falls bot haengenbleibt */
    > +void bot_check_hang_on_behaviour(Behaviour_t *data);
    > +
    > +/*! Verhalten zum echten Abfahren des bots nach der Punkte-Wegeliste laut Pfadplanung zum global gesetzten Ziel*/
    > +void bot_gotoxy_behaviour_map(Behaviour_t *data);
    > +void bot_gotodest_map(Behaviour_t *caller);
    > +
    > +
    > +/*! Display zum Aufruf der Mapgo-Routinen */
    > +void mapgo_display(void);
    > +
    > +/*! Der Punkt xy befindet sich auf den Zielkoordinaten oder innerhalb eines Umkreises davon  */
    > +int8 map_in_dest (uint16 x, uint16 y, uint16 destx, uint16 desty);
    > +
    > +#define MAPFIELD_IGNORE          80 /*!< negativer Schwellwert, bei dem Laut Map Hindernis gemeldet wird */
    > +#define MAP_ART_HAZARD           100 /*!< kuenstliches Hindernis in MAP fuer Pathplaning */
    > +
    > +
    > +#endif /* BEHAVIOUR_MAP_GO_DESTINATION_H_ */
    > +#endif /* BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE */
    >
    >
    > ------------------------------------------------------------------------
    >
    > _______________________________________________
    > ct-bot-entwickler Mailingliste
    > ct-bot-entwickler@xxxxxxxxxxxxxxxxx
    > http://www.heise.de/bin/newsletter/listinfo/ct-bot-entwickler

    --
    Benjamin Benz
    Heise Zeitschriften Verlag
    Redaktion c't
    eMail: bbe@xxxxxxxx
    WWW  : http://www.heise.de

    Heise Zeitschriften Verlag GmbH & Co. KG
    Registergericht: Amtsgericht Hannover HRA 26709

    Persönlich haftende Gesellschafterin:
    Heise Zeitschriften Verlag Geschäftsführung GmbH
    Registergericht: Amtsgericht Hannover, HRB 60405
    Geschäftsführer: Ansgar Heise, Steven P. Steinkraus, Dr. Alfons Schräder

- Antworten:
  - AW: [ct-bot] neues Pfadplanungs-Verhalten nach der Map
    - From: Frank Menzel

- Referenzen:
  - [ct-bot] neues Pfadplanungs-Verhalten nach der Map
    - From: Frank Menzel

- Voriger (Datum): [ct-bot] neues Pfadplanungs-Verhalten nach der Map, Frank Menzel
- Nächster (Datum): Re: [ct-bot] neues Pfadplanungs-Verhalten nach der Map, Benjamin Benz
- Voriger (Thread): [ct-bot] neues Pfadplanungs-Verhalten nach der Map, Frank Menzel
- Nächster (Thread): AW: [ct-bot] neues Pfadplanungs-Verhalten nach der Map, Frank Menzel
- Indizes:
  - Datum
  - Thread