## c't-Projekte - Mailinglisten

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

### [ct-bot] neues Pfadplanungs-Verhalten nach der erstellten/ online erstellten Karte

Absender: Frank Menzel
Datum: Fr, 09.02.2007 22:12:00

    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.7
    diff -u -r1.7 .cdtbuild
    --- .cdtbuild	8 Jul 2006 02:36:15 -0000	1.7
    +++ .cdtbuild	9 Feb 2007 21:14:06 -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.128
    diff -u -r1.128 Changelog.txt
    --- Changelog.txt	29 Jan 2007 18:22:06 -0000	1.128
    +++ Changelog.txt	9 Feb 2007 21:14:07 -0000
    @@ -1,5 +1,9 @@
     CHANGELOG fuer c't-Bot
     ======================
    +2007-01-21  Frank Menzel(menzelfr@xxxxxx) Map-Routinen mit Pfadplanung; dazu Screen 1 (Red) zum Aufruf der Map-Verhalten belegt; 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 5 gibt
    +                        die Punkteliste laut Pfadplaner zum Ziel auf die Konsole aus
    +
     2007-01-29 Benjamin Benz [bbe@xxxxxxxx]: Map wird jedesmal neu gesucht, wenn die MMC-Seite offen ist UND sich der Init-Status der Karte aendert

     2007-01-29 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Bugfix in mini-fat.c - sofortiger Abbruch bei Fehler
    Index: map.c
    ===================================================================
    RCS file: /ctbot/ct-Bot/map.c,v
    retrieving revision 1.3
    diff -u -r1.3 map.c
    --- map.c	15 Jan 2007 11:08:37 -0000	1.3
    +++ map.c	9 Feb 2007 21:14:08 -0000
    @@ -66,7 +66,7 @@
     #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			70	/*!< 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 */
    Index: rc5.c
    ===================================================================
    RCS file: /ctbot/ct-Bot/rc5.c,v
    retrieving revision 1.28
    diff -u -r1.28 rc5.c
    --- rc5.c	22 Jan 2007 13:08:35 -0000	1.28
    +++ rc5.c	9 Feb 2007 21:14:09 -0000
    @@ -434,6 +434,40 @@
     				break;
     		#endif
     		default:
    +		//-- Map-Routinen auf Screen 1 --
    +
    +		// Screen 1 dient fuer die Map-Routinen
    +		#ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    +		  switch (display_screen) {
    +		  	case 1:
    +		  	  switch (par->value1) {
    +				    case 0:
    +					      target_speed_l=0;target_speed_r=0;break;
    +
    +					case 1:
    +					       //Zielkoords relativ zu Botposition
    +					       bot_gotoxy(0,180,-700); //
    +					       //bot_gotoxy_map(0,-9,15);
    +					       break;  //lbot bischen links oberhalb startpos 83,76
    +					case 2: bot_gotoxy_map(0,9,-15);break;  //weit links oben 89,65
    +					case 3: bot_set_destination(0);break;  // Standort des Bots als Ziel festlegen
    +					case 4: bot_gotodest_map(0);break;     // Gehe zum Ziel nach Pfadplaner und def. Endziel
    +					case 5: bot_path_bestfirst(0);break;   // nur Aufruf des Pfadplaner mit Pfadausgabe
    +					case 6:
    +					       #ifdef PC
    +					          show_labmap();
    +					       #endif
    +					       break;
    +					case 7: bot_scan(0); break;
    +					case 8: print_map(); break;
    +					case 9:              break;
    +		       }
    +		      break;
    +
    +			default:
    +
    +		#endif  // BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    +
     	#endif
     			switch (par->value1) {
     				#ifdef BEHAVIOUR_AVAILABLE
    @@ -492,6 +526,11 @@

     //				case 9:	 target_speed_l=-BOT_SPEED_MEDIUM;target_speed_r=target_speed_l;break;
     			}
    +		#ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    +		  break;
    +		  }  // Ende des switch default-Zweiges
    +		#endif
    +
     	#ifdef DISPLAY_SCREENS_AVAILABLE

     			break;
    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	9 Feb 2007 21:14:09 -0000
    @@ -1,4 +1,4 @@
    -#Fri Apr 28 20:32:38 CEST 2006
    +#Wed Feb 07 19:58:50 CET 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
     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: bot-logic/bot-logik.c
    ===================================================================
    RCS file: /ctbot/ct-Bot/bot-logic/bot-logik.c,v
    retrieving revision 1.2
    diff -u -r1.2 bot-logik.c
    --- bot-logic/bot-logik.c	15 Jan 2007 11:07:33 -0000	1.2
    +++ bot-logic/bot-logik.c	9 Feb 2007 21:14:10 -0000
    @@ -100,7 +100,7 @@

     	#ifdef BEHAVIOUR_SCAN_AVAILABLE
     		// Verhalten, das die Umgebung des Bots on-the fly beim fahren scannt
    -		insert_behaviour_to_list(&behaviour, new_behaviour(155, bot_scan_onthefly_behaviour,ACTIVE));
    +		insert_behaviour_to_list(&behaviour, new_behaviour(203, bot_scan_onthefly_behaviour,ACTIVE));

     		// Verhalten, das einmal die Umgebung des Bots scannt
     		insert_behaviour_to_list(&behaviour, new_behaviour(152, bot_scan_behaviour,INACTIVE));
    @@ -122,6 +122,13 @@
     	#ifdef BEHAVIOUR_GOTOXY_AVAILABLE
     		insert_behaviour_to_list(&behaviour, new_behaviour(147, bot_gotoxy_behaviour,INACTIVE));
     	#endif
    +
    +        // Verhalten, um laut Map zu einem bestimmten Ziel zu fahren
    +        #ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    +          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(135, bot_gotoxy_behaviour_map,INACTIVE));
    +        #endif

     	#ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE
    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	9 Feb 2007 21:14:10 -0000
    @@ -26,6 +26,12 @@

     #define BEHAVIOUR_REMOTECALL_AVAILABLE /*!< Nehmen wir Remote-kommandos entgegen?*/

    +//#ifdef PC
    +  // geht leider nicht auf dem echten bot
    +  #define BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE	/*! MAP-Verhalten Ausgangssuche nach Best-First vorhanden ?*/
    +//#endif
    +
    +
     /* 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
    @@ -93,6 +99,9 @@
     #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	9 Feb 2007 21:14:10 -0000
    @@ -45,6 +45,8 @@
      * @param y Y-Ordinate an die der Bot fahren soll
      */
     void bot_gotoxy(Behaviour_t *caller, float x, float y);
    +
    +int16 bot_gotoxy_calc_turn(float xDiff, float yDiff);
     #endif

     #endif /*BEHAVIOUR_GOTOXY_H_*/
    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,1387 @@
    +/*
    + * 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 geht nach der erstellten Karte (bzw. wird online erstellt) zu einer bestimmten
    + *          Zielkoordinate und umgeht durch vorheriger Pfadplanung nach best-first und der
    + *          Potenzialfeldmethode vorhandene Hindernisse; die Zielkoordinate ist zu Beginn die
    + *          Startposition und kann mittels Tastendruck(3)im gesonderten Verhalten auf aktuelle
    + *          Botposition geaendert werden;
    + *          Die bordeigenen Sensoren weren hier in keiner Art und Weise benutzt, es wird
    + *          ausschliesslich nach der Map gefahren !
    + *          Auf dem echten bot habe ich das Verhalten bei mir leider nicht zum laufen gebracht,
    + *          es kommt zum sofortigen Blinken mit aktiviertem Define, auf dem PC problemlos.
    + *
    + *  @author 	Frank Menzel (-alias achiem -Menzelfr@xxxxxxx)
    + *  @date 	03.01.07
    +*/
    +
    +
    +#include "bot-logic/bot-logik.h"
    +#include "bot-logic/behaviour_map_go_destination.h"
    +#include "map.h"
    +#include <stdlib.h>
    +#include <math.h>
    +
    +
    +#ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    +
    +/*! Liste mit allen Nodes, die zur Pfadplanung nach Best-First Verwendung finden */
    +Node_t *nodes = NULL;
    +
    +/*! Knoten, der immer als naechstes Botziel dient */
    +Node_t *node_nextdest=NULL;
    +
    +/*! 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 CIRCLE_RADIUS			70  //in mm, im Umfeld von x-Mapfelder wird Umkreis gelegt und Potenzial untersucht; ist dann auch bot-Schrittweite
    +#define CIRCLE_RADIUS_MAP_FIELDS	(MAP_RESOLUTION*CIRCLE_RADIUS/1000)
    +
    +/*! Botradius in Map-Felder umgerechnet je nach Aufloesung; erst aus map.c/h genommen, wurde dort aber geaendert und daher hier fest */
    +#define MAP_RADIUS_GODEST			  80 // 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]*/
    +#define MAP_RADIUS_FIELDS_GODEST_QUAD (MAP_RADIUS_FIELDS_GODEST * MAP_RADIUS_FIELDS_GODEST) // Quadrat des Radiusumkreises fuer direkten Vergleich
    +
    +#define MAXCOUNT_SAMEPOS 250     // Abbruchcounter, falls bot haengenbleibt
    +#define MAXCOUNTER 300           // Zielsuche bis zu dieser Tiefe, hier wird Routine spaetestens beendet
    +
    +/*! Werte, wie bestimmte Mapfelder angezeigt werden zur Visualisierung bei geringer Aufloesung auf dem PC */
    + #define SPACE 0           // Leer- Initialfeld
    + #define WALL  1           // Wandfeld
    + #define HOLE  7           // Abgrund, Loch
    + #define BOTPOS  3         // Botposition selbst
    + #define PATHNODES  9      // aus Pfadplanung
    +
    +
    +/*! globale Zielkoordinaten fuer Pfadplanung; ist zuerst immer das Bot-Map-Startfield */
    +volatile uint16 dest_x_map=(MAP_SIZE*MAP_RESOLUTION/2);
    +volatile uint16 dest_y_map=(MAP_SIZE*MAP_RESOLUTION/2);
    +
    +/*! Zustandsvar des Verhaltens nach aussen gezogen, um dieses im Botenverhalten korrekt init. zu koennen;nach Notaus
    + *  koennte diese sonst einen undef. Zustand haben
    + */
    +static int8 gotoState=0;
    +
    +/*! Koordinaten des die Hindernisse umgebenden Rechtecks; das Rechteck der Koords von SHRINK_ONLINE ist zu gross, da
    + *  fuer jene Koords auch die Mapkoords drin sind mit Wert 0 ohne Hindernis in Abstand der Sensoren; dies ist die
    + *  Ermittlung der Koords aus SHRINK_OFFLINE, also das knappeste Rechteck um die Hindernisse
    + */
    +uint16 map_xmin=0;
    +uint16 map_xmax=0;
    +uint16 map_ymin=0;
    +uint16 map_ymax=0;
    +
    +// ===== Routinen fuer die vorwaerts verkettete Liste mit den Nodes fuer die Pfadplanung =====  //
    +
    +/*! Setzen der Eigenschaften des Nodes
    + *  @param xp yp x- und y-Koordinate; direkter Array-Wert der Map
    + *  @param CoastToGoal Potenzial des Nodes als Summe der anziehenden und abstossenden Potenziale
    + *  @param exists_in_queue Kennung auf Zugehoerigkeit zur logischen Liste A oder B; True fuer B
    + *  @param visited Kennung ob Punkt schon untersucht wurde, bei True wird globales Minimum erkannt
    + *  @param node die Eigenschaften dieses Knotens sind zu setzen
    + *  @param parent_pathnode fuer backtracking vom Zielnode ist dies der vorige anzufahrende Knoten
    + *  @param next wegen vorwaerts verkettete Liste ist dies der Folgenode
    + */
    +void set_node(uint16 xp, uint16 yp, float CoastToGoal,
    +                 int8 exists_in_queue, int8 visited, Node_t *node, Node_t *parent_pathnode, Node_t *next){
    +
    +	if (node == NULL)
    +		return ;
    +
    +	node->xpos = xp;   // jeder Node ist eindeutig durch seine x-y-Koordinate gekennzeichnet
    +	node->ypos = yp;
    +
    +    node->parent_pathnode=parent_pathnode;  // Elternnode zur Pfad-Rueckverfolgung
    +
    +	node->TotalCoast  = CoastToGoal;  // Summe der Gesamtkosten
    +
    +	node->exists_in_queue = exists_in_queue;  //Kennung zu welcher Liste gehoerig
    +	node->visited = visited;  // Kennung auf besucht
    +
    +	node->next= next;  // fuer vorwaerts verkettete Liste Folgenode
    +
    +}
    +
    +
    +/*! Erzeugung eines neuen Nodes */
    +Node_t *new_node(uint16 xp, uint16 yp, float CoastToGoal,
    +                 int8 exists_in_queue, int8 visited, Node_t *parnode_path){
    +
    +	Node_t *newnode = (Node_t *) malloc(sizeof(Node_t));
    +
    +	if (newnode == NULL)
    +		return NULL;
    +
    +	set_node(xp,yp,CoastToGoal,exists_in_queue,visited,newnode,parnode_path,NULL);
    +
    +	return newnode;
    +}
    +
    +
    +
    +/*! Fuegt einen Node in die Liste ein, aufsteigend sortiert nach Kosten der unbesuchten Nodes
    + *  @param **list Liste mit den Knoten
    + *  @param *node  Knoten, der sortiert in die Liste A eingefuegt werden soll
    + */
    +void insert_node_to_list(Node_t **list, Node_t *node){
    +	Node_t	*ptr	= *list;
    +	Node_t *temp	= NULL;
    +
    +	/* Null-Node gibts nicht */
    +	if (node == NULL)
    +		return;
    +
    +	/* Erster Eintrag in der Liste? */
    +	if (ptr == NULL){
    +		ptr = node;
    +		*list = ptr;
    +	} else {
    +		/* Gleich mit erstem Eintrag tauschen, falls aktuelles Potential kleiner ist */
    +		// und weder in B noch besucht ist; denn sortiert muessen nur A und besuchte nodes sein
    +		if (!ptr->exists_in_queue && !ptr->visited && ptr->TotalCoast > node->TotalCoast ) {
    +			node->next = ptr;  // Null wieder eintragen
    +			ptr=node;
    +			return;
    +		} else {
    +		    temp=ptr;
    +			/* Mit dem naechsten Eintrag vergleichen in A */
    +			while(NULL != ptr->next) {
    +				if (!ptr->exists_in_queue && !ptr->visited && ptr->TotalCoast > node->TotalCoast)	{
    +				temp->next=node;
    +				node->next = ptr;
    +			    return;
    +
    +				}  //if
    +				// Naechster Eintrag
    +				temp = ptr;
    +				ptr = ptr->next;
    +			}  //while
    +
    +
    +			if (!ptr->exists_in_queue && !ptr->visited && ptr->TotalCoast > node->TotalCoast)	{
    +				node->next = ptr;
    +				temp->next = node;
    +				ptr->next=NULL;
    +			    return;
    +
    +				}  //if
    +			ptr->next = node;   // Node in die Liste hinten anfuegen
    +			node->next = NULL;  // Null wieder eintragen
    +
    +		}
    +	}
    +}
    +
    +/*! Rueckgabe des ersten Knotens der Liste A, der auch noch nicht besucht wurde und damit
    + *  das kleinste Potenzial (Kosten) hat
    + *  @param **list Liste mit den Knoten
    + */
    +Node_t *get_first_node_from_list(Node_t **list){
    +	Node_t	*ptr	= *list;
    +
    +			/* ersten Node aus A zurueckgeben wenn einer vorhanden, da diese
    +			   Liste A bereits sortiert ist */
    +			while(NULL != ptr ) {
    +				if (!ptr->exists_in_queue && !ptr->visited)
    +					return ptr;
    +				// Naechster Eintrag
    +				ptr = ptr->next;
    +			}
    +	return NULL;
    +}
    +
    +/*! Rueckgabe des letzten Nodes der Liste; dieser ist der gefundene Endziel-Punkt mit Potenzial 0
    + *  node ist nicht unbedingt identisch den Zielkoordinaten, falls Aufloesung hoch und im Umkreis; ab diesem
    + *  wird via backtracking der naechste anzufahrende Punkt ermittelt
    + *  @param **list Liste mit den Nodes
    + */
    +Node_t *get_last_node_from_list(Node_t **list){
    +	Node_t	*ptr	= *list;
    +
    +			while(NULL != ptr && ptr->TotalCoast!=0 ) {
    +				// Liste bis zum Ende durchgehen
    +				ptr = ptr->next;
    +			}
    + 	return ptr;
    +}
    +
    +
    +/*! Rueckgabe des Knotens mit den Map-Koordinaten xy wenn vorhanden
    + *  @param **list Liste mit den Nodes
    + *  @param xy Map-Koordinaten des zu suchenden Nodes
    + *  @return Node mit uebereinstimmender xy-Koordinate
    + * */
    +Node_t *node_exists_in_list(Node_t **list, uint16 x, uint16 y){
    +	Node_t	*ptr	= *list;
    +            // Liste durchgehen bis Punkt gefunden; hier ist noch Optimierungspotenzial: die Liste
    +            // wird hier sequentiell durchlaufen, liegt sie komplett sortiert nach TotalCoast vor,
    +            // im Moment nur fuer Liste A, kann rekursiv nach Motto: Teile-und-herrsche gesucht werden.
    +			while(NULL != ptr ) {
    +
    +				if (ptr->xpos == x && ptr->ypos == y)
    +					return ptr;
    +
    +				// Naechster Eintrag
    +				ptr = ptr->next;
    +			}
    +	return NULL;
    +}
    +
    +/*! Leeren der Knoten der Liste mit Speicherfreigabe und NULL-Setzen der Liste
    + *  @param **list Liste mit den Nodes
    + */
    +void nodes_free(Node_t **list){
    +	Node_t	*ptr	= *list;
    +	Node_t *nt=NULL;
    +
    +			while(NULL != ptr ) {
    +			    nt = ptr->next;
    +
    +				free((Node_t *)ptr);
    +				ptr=nt;
    +			}
    +
    +	*list=NULL;
    +}
    +
    +
    +
    +/*! zeigt die Pfadliste der Punkte mit den Zielkoordinaten nach Pfadplanung;
    + *  es wird die globale node-Variable node_nextdest belegt mit dem naechsten anzufahrenden Punkt
    + *  @param **liste Liste mit den Nodes
    + *  @param dest_x_map und dest_y_map  globale Zielkoordinaten
    + *  @param forward Kennung, ob Planung vom Start zum Endziel erfolgte oder umgedreht
    + */
    +void show_list_destkoords(Node_t **list, uint16 dest_x_map, uint16 dest_y_map, int8 forward){
    +    Node_t *n_act=NULL;
    +
    +    uint16 startx = world_to_map(x_pos);
    +    uint16 starty = world_to_map(y_pos);
    +
    +        //gefundenen Zielknoten ermitteln mit Potential 0
    +        n_act=get_last_node_from_list(&nodes);
    +
    +        node_nextdest=NULL;
    +
    +        #ifdef PC	// Ausgabe nur fuer PC
    +         while (n_act!=NULL /*&& node_nextdext==NULL*/) {
    +            printf("Path xy: %2d %2d, Bot: %1d %1d, Start: %1d %1d \n",n_act->xpos,n_act->ypos,startx,starty,dest_x_map,dest_y_map);
    +
    +        #else  // fuer MCU, also dem echten bot, nur naechsten Node bestimmen und dann Schluss
    +         while (n_act!=NULL && node_nextdext==NULL ) {
    +        #endif
    +
    +             if (n_act!=NULL && node_nextdest==NULL) {
    +              // den ersten Eintrag vom Zielnode aus nehmen, der sich nicht im Umkreis befindet	und nicht der letzte ist
    +              if (!map_in_dest (n_act->xpos,n_act->ypos,startx, starty) && n_act->parent_pathnode!=NULL) {
    +                node_nextdest=n_act;
    +                #ifdef PC
    +                  printf("erstes bot-Zwischenziel: %1d %1d \n",node_nextdest->xpos, node_nextdest->ypos);
    +                #endif
    +              }  // if map_in_dest
    +            }
    +
    +        	n_act=n_act->parent_pathnode;  // weiter langhangeln
    +        }  //while
    +
    +}
    +
    +
    +// ============= Ende der Node- und Listdefinitionen mit entsprechenden Routinen =============
    +
    +/*!
    + * Die Funktion bestimmt die Map-Eckkoordinaten des die Hindernisse knappesten umgebenden Rechteckes.
    + * Die online gesetzten Werte bei SHRINK_ONLINE ergeben aber aktuell nicht das knappste Rechteck um
    + * die Hindernisse, werden aber trotzdem genommen, was aber zu Overhead fuehren koennte.
    + * @param min_xp Minimalste x-Koord
    + * @param max_xp Maximalste x-Koord
    + * @param min_yp Minimalste y-Koord
    + * @param max_yp Maximalste y-Koord
    + */
    +void get_rectcoords_environment(uint16 *min_xp,uint16 *max_xp, uint16 *min_yp, uint16 *max_yp) {
    +	  // Eckpunkte sind die online aktualiserten globalen Variablen laut map.h fuer SHRINK_ONLINE
    +	  *min_xp = map_min_x;
    +	  *max_xp = map_max_x;
    +	  *min_yp = map_min_y;
    +	  *max_yp = map_max_y;
    +}
    +
    +
    +
    +// ============= Routinen zur Ermittlung der Potenziale =======
    +
    +/*! Ermittlung des Abstandes zwischen 2 Koordinaten; verwendet zur Potenzial-Abstandsermittlung zum Zielpunkt
    + * @param xs x-Koord des zu untersuchenden Punktes
    + * @param ys y-Koord des zu untersuchenden Punktes
    + * @param xd x-Koord des Zielpunktes
    + * @param yd y-Koord des Zielpunktes
    + * @return liefert Quadrat-Abstand zwischen den Map-Punkten als float-Wert zur Potenzialbestimmung
    + */
    +float get_dist(int16 xs, int16 ys, int16 xd, int16 yd){
    +	//printf("in get_dist: %1d %1d %1d %1d \n",xs, ys,xd,yd);
    +	int16 xt=xd - xs;
    +	int16 yt=yd-ys;
    +
    +	/* Abstandsermittlung nach dem guten alten Pythagoras ohne Ziehen der Wurzel */
    +	return xt*xt+yt*yt;
    +
    +}
    +
    +
    +
    +/*! am Rand ist die Abstossungskraft und damit das Potenzial sehr hoch, damit im Labyrinth die Raender
    + *  benachteiligt werden
    + * @param x x-Koord des zu untersuchenden Punktes
    + * @param y y-Koord des zu untersuchenden Punktes
    + * @return hoehes repulsives Potenzial ausserhalb der Grenzen, sonst 0
    + */
    +int8 get_border_rep (uint16 x, uint16 y) {
    +   if (y>map_ymax)
    +      return 1;  // hohes Pot. ausserhalb des Konfigurationsraumes, sprich Umgebungsechteck der Hindernisse
    +    else
    +      return 0;  // innerhalb des Konfig-raumes ist Summand 0
    +}
    +
    +
    +/*! 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
    + *  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
    + * @return repulsives Potenzial fuer Map-Hinderniswahrscheinlichkeit
    + */
    +float get_rep_pot(uint16 x, uint16 y) {
    +	#define OFF_DEF 127  // bei 0 zurechnen -Achtung Wertebereich von -128- + 127 !
    +
    +	// Bereich ist von -128 (Loch) bis +127 (am freiesten, da bereits befahren)
    +	//in positiven Bereich transformieren
    +	int8 tmp=map_get_field(x,y);
    +
    +	//bei 0 ist es unerforschtes Gebiet und ist benachteiligter, genauso frei behandeln wie erforschtes freies Gebiet
    +	#if OFF_DEF !=0
    +	 if (tmp==0)
    +	  tmp=tmp+OFF_DEF;
    +	#endif
    +
    +	float tmpf=tmp+131;   // 128 + 3 damit mindestens 3 entsteht
    +
    +	return 1/tmpf;
    +
    +
    +}
    +
    +
    +/*! ermittelt das gefaltete Potential an der Pos. xy; Belegungswahrscheinlichkeit haengt ebenfalls
    + *  ab von einem moeglichen Hindernis vor oder nach dieser Position, daher wird bei Hindernis drumrum das
    + * Belegungspotential erhoeht wodurch Wahrscheinlichkeit der Verwendung sinkt
    + * @param x x-Koord des zu untersuchenden Punktes
    + * @param y y-Koord des zu untersuchenden Punktes
    + * @return gefaltetes Potenzial der direkten 8 Umgebungszellen
    +*/
    +float get_rep_folded_pot(uint16 x, uint16 y) {
    +	float pot=0;
    +	float potrep=0;
    +	int16 tmp;
    +
    +	//zeilenweise Faltung
    +	if (x-1 > map_xmin) {
    +	  tmp=map_get_field(x-1,y);
    +	  if (tmp<0) {//Hinderniswahrscheinlichkeit ist < 0
    +	  	//Abstossungspotential bei x-1 ermitteln
    +	  	potrep=get_rep_pot(x-1,y);
    +	  	//nur mit 1Viertel beruecksichtigen
    +	  	pot=potrep / 4;
    +	  }
    +	}
    +
    +	  //gleiches an Stelle x+1
    +	  if (x+1 < map_xmax-1 ) {
    +	  tmp=map_get_field(x+1,y);
    +	  if (tmp<0) {//Hinderniswahrscheinlichkeit ist < 0
    +	  	//Abstossungspotential bei x-1 ermitteln
    +	  	potrep=get_rep_pot(x+1,y);
    +	  	//nur mit 1Viertel beruecksichtigen und aus Pos-1 zurechnen
    +	  	pot=potrep / 4 + pot;
    +	  }
    +	}
    +
    +
    +	//spaltenweise Faltung
    +	if (y-1 > map_ymin) {
    +	  tmp=map_get_field(x,y-1);
    +	  if (tmp<0) {//Hinderniswahrscheinlichkeit ist < 0
    +	  	//Abstossungspotential bei x-1 ermitteln
    +	  	potrep=get_rep_pot(x,y-1);
    +	  	//nur mit 1Viertel beruecksichtigen
    +	  	pot=potrep / 4;
    +	  }
    +	}
    +
    +	if (y+1 < map_ymax-1) {
    +	  //gleiches an Stelle x+1
    +	  tmp=map_get_field(x,y+1);
    +	  if (tmp<0) {//Hinderniswahrscheinlichkeit ist < 0
    +	  	//Abstossungspotential bei x-1 ermitteln
    +	  	potrep=get_rep_pot(x,y+1);
    +	  	//nur mit 1Viertel beruecksichtigen und aus Pos-1 zurechnen
    +	  	pot=potrep / 4 + pot;
    +	  }
    +	}
    +
    +
    +
    +	//diagonale Felder ebenfalls beruecksichtigen
    +	if (x-1 > map_ymin  && y-1 > map_ymin) {
    +	  tmp=map_get_field(x-1,y-1);
    +	  if (tmp<0) {//Hinderniswahrscheinlichkeit ist < 0
    +	  	//Abstossungspotential ermitteln
    +	  	potrep=get_rep_pot(x-1,y-1);
    +	  	//nur mit 1Achtel beruecksichtigen
    +	  	pot=potrep / 8;
    +	  }
    +	}
    +
    +	if (x-1 > map_ymin && y+1 < map_ymax-1) {
    +	  tmp=map_get_field(x-1,y+1);
    +	  if (tmp<0) {//Hinderniswahrscheinlichkeit ist < 0
    +	  	//Abstossungspotential ermitteln
    +	  	potrep=get_rep_pot(x-1,y+1);
    +	  	//nur mit 1Achtel beruecksichtigen
    +	  	pot=potrep / 8;
    +	  }
    +	}
    +
    +	if (x+1 < map_xmax-1 && y-1 > map_ymin) {
    +	  tmp=map_get_field(x+1,y-1);
    +	  if (tmp<0) {//Hinderniswahrscheinlichkeit ist < 0
    +	  	//Abstossungspotential ermitteln
    +	  	potrep=get_rep_pot(x+1,y-1);
    +	  	//nur mit 1Achtel beruecksichtigen
    +	  	pot=potrep / 8;
    +	  }
    +	}
    +
    +	if (x+1 < map_xmax-1 && y+1 < map_ymax-1) {
    +	  tmp=map_get_field(x+1,y+1);
    +	  if (tmp<0) {//Hinderniswahrscheinlichkeit ist < 0
    +	  	//Abstossungspotential ermitteln
    +	  	potrep=get_rep_pot(x+1,y+1);
    +	  	//nur mit 1Viertel beruecksichtigen
    +	  	pot=potrep / 8;
    +	  }
    +	}
    +
    +	return pot;
    +
    +}
    +
    +
    +
    +/*! 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
    + */
    +float get_att_pot(uint16 x, uint16 y, uint16 xdest, uint16 ydest) {
    +// Als Potenzialfunktion dient die Bestimmung des Abstandes, 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
    +//bei Aufloesung in Botbreite ist Abstand 12,5cm von einer Mapkoord zur naechsten
    +	float dist;
    +    // Teiler wird hoeher mit hoeherer Aufloesung, um den Wert in kleinem Wertebereich zu halten
    +    #if MAP_RADIUS_FIELDS_GODEST <= 1  // MCU-Aufloesung
    +      #define DIV 1000
    +    #else
    +      #define DIV (100000 * MAP_RADIUS_FIELDS_GODEST)  //in Abhaengigkeit des Such-Umkreises
    +    #endif
    +
    +	// Abstand berechnen mit Mapaufloesung und Mapkoordinaten; dieser wurde oben Wurzelziehen ermittelt
    +	dist=get_dist(x,y,xdest,ydest);
    +    return dist/DIV;  //einfach mal Faktor gewaehlt zum teilen
    +
    +}
    +
    +
    +/*! liefert Summe aller repulsiven und attraktiven Potentiale, die am kleinsten ist fuer den best passendsten
    + *  Knoten nach Potentialfeldmethode
    + */
    +float get_all_pot(uint16 xs, uint16 ys, uint16 xd, uint16 yd) {
    +
    +	float sumpot=0;
    +	float reppot=0;
    +	float attpot=0;
    +	float borderpot=0;
    +    float foldedpot=0;
    +
    +	//falls der zu untersuchende Knoten der Zielknoten ist, keine Abstossungskraft, lok. Minimum
    +	if   (xs==xd && ys==yd)
    +	  sumpot=0;
    +	  else {
    +	    reppot=get_rep_pot(xs,ys);
    +	    attpot=get_att_pot(xs,ys,xd,yd);
    +	    borderpot= get_border_rep(xs,ys);
    +        foldedpot= get_rep_folded_pot(xs,ys);
    +        // Einzelpotenziale zum Gesamtwert addieren
    +	    sumpot=reppot + attpot + borderpot + foldedpot;
    +	  }
    +
    +	return sumpot;
    +}
    +
    +
    +/*! liefert nur die Summe aller abstossenden Potentiale, also ohne anziehendes Zielpotenzial
    + */
    +float get_all_rep_pot(uint16 xs, uint16 ys, uint16 xd, uint16 yd) {
    +
    +	float 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);  //Gefahrenpot. auf Zelle selbst
    +
    +	sumpot=sumpot + get_border_rep(xs,ys);
    +	return sumpot + get_rep_folded_pot(xs,ys);
    +
    +}
    +
    +
    +#ifdef PC
    +/*! Hilfsroutine zur Kartenausgabe; 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_ymin; y<= map_ymax; y++) {
    +		    printf("xy: %2d %2d :", map_xmin+1,y);
    +
    +			  for (x=map_xmax; x> map_xmin; 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 = PATHNODES; //5
    +
    +				if (x_posw==x && y_posw==y)
    +				    tmpr=BOTPOS;
    +
    +				printf("%1d|",tmpr);
    +			}
    +		    printf("\n");
    +
    +		}
    +
    +		//noch einmal mit konkreten Werten
    +	/*	 for (y=map_ymin; y< map_ymax; y++) {
    +		//for (y=map_min_y; y< map_max_y; y++) {
    +		    printf("xy:%2d %2d : ",map_xmin-1,y);
    +			//for (x=map_min_x; x<map_max_x; x++){
    +			    for (x=map_xmax; x>= map_xmin; 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");
    +		}
    +	*/
    +
    +	}
    +
    +#endif
    +
    +
    +
    +
    +// ======================= Beginn der Pfadfolge-Routinen ====================================
    +
    +/*!
    + * Check, ob der Punkt xy mit destxy identisch ist (bei kleinen Aufloesungen) oder sich
    + * innerhalb des Radius-Umkreises befindet (hoehere Aufloesungen)
    + * @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!!!)
    + */
    +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_QUAD;
    +
    +  #else
    +    //bei geringer Aufloesung (MCU) direkt mit Zielkoords vergleichen
    +  	return  (x==destx && y==desty);
    +  #endif
    +}
    +
    +/*! Verhalten, damit der bot der Nodes-Pfadlinie folgt
    + *  das Verhalten bot_goto_xy geht nicht mit den negativen Koordinaten, daher ein eigenes Verhalten
    + *  mit den nur positiven Map-Koordinaten
    + *  @param *data der Verhaltensdatensatz
    + */
    +void bot_gotoxy_behaviour_map(Behaviour_t *data){
    +	#define INITIAL_TURN 	0
    +	#define GOTO_LOOP 	    1
    +	#define CORRECT_HEAD	2
    +	#define POS_REACHED	    3
    +	#define NEXT_POS	    4
    +	#define INITIAL_GETPATH 5
    +	#define GO_PATH 	    6
    +
    +	static uint16 diffx_last=0;
    +	static uint16 diffy_last=0;
    +
    +	static uint8 cancelcounter=0;
    +	static uint16 cancelcounter_samepos=0;
    +
    +	int16 X_pos;
    +	int16 Y_pos;
    +	float xDiff=0;
    +	float yDiff=0;
    +    int8 fieldval=0;
    +    static int16 target_x=0;				/*!< Zielkoordinate X */
    +    static int16 target_y=0;				/*!< Zielkoordinate Y */
    +
    +	//Mauskoordinaten selbst
    +	X_pos=world_to_map(x_pos);
    +	Y_pos=world_to_map(y_pos);
    +
    +
    +	//Jederzeit Schluss wenn Positionen Maus Endziel uebereinstimmen
    +	 if (map_in_dest (X_pos, Y_pos,dest_x_map, dest_y_map)) {
    +		gotoState=POS_REACHED;
    +		speedWishLeft=BOT_SPEED_STOP;
    +		speedWishRight=BOT_SPEED_STOP;
    +	}
    +
    +	if (node_nextdest!=NULL && map_in_dest (X_pos,Y_pos,dest_x_map, dest_y_map)) {
    +	      gotoState=POS_REACHED;
    +	}
    +
    +
    +	 if (node_nextdest!=NULL && gotoState!=POS_REACHED) {
    +
    +	 	// bei hoeherer Aufloesung muss hier voraus im Botdurchmesser auf Hindernis gecheckt werden
    +	 	// liegt im Umkreis des naechsten Punktes ein Hindernis, neue Wegplanung erforderlich
    +
    +	 	fieldval=map_get_field(node_nextdest->xpos,node_nextdest->ypos);
    +	 	if (fieldval<=-MAPFIELD_IGNORE  ) {
    +	      //wenn naechster Punkt auch Endziel dann Schluss
    +	       if (map_in_dest (node_nextdest->xpos, node_nextdest->ypos,dest_x_map, dest_y_map)) {
    +	         gotoState=POS_REACHED;
    +	       }
    +	       else
    +	         gotoState=INITIAL_GETPATH;
    +
    +		  speedWishLeft=BOT_SPEED_STOP;
    +		  speedWishRight=BOT_SPEED_STOP;
    +	 	}
    +	 }
    +
    +	// hier nun Aufruf der Funktionen nach den Verhaltenszustaenden
    +	switch(gotoState) {
    +		case INITIAL_GETPATH:
    +		    // Pfadplaner aktuell nur beim ersten Mal aufgerufen, danach wird sich am
    +		    // Punkt langgehangelt; dynamische Hindernisse werden erst mal nicht beruecksichtigt
    +		    bot_path_bestfirst(0);
    +
    +		    gotoState=GO_PATH;
    +			cancelcounter_samepos=0;
    +
    +			break;
    +
    +		case GO_PATH:
    +		    // Pfadplanung muss Ergebnis liefern, sonst gehts eben nicht weiter
    +		    cancelcounter_samepos=0;
    +
    +		    if (node_nextdest!=NULL && map_in_dest(X_pos,Y_pos,node_nextdest->xpos,node_nextdest->ypos)) {
    +				 gotoState=POS_REACHED;
    +			}
    +
    +		    if   (node_nextdest!=NULL) {
    +		      target_x = node_nextdest->xpos;
    +		      target_y = node_nextdest->ypos;
    +		      xDiff=target_x-X_pos;
    +	          yDiff=target_y-Y_pos;
    +
    +			  gotoState=INITIAL_TURN;
    +			  cancelcounter=0;
    +		    }
    +		    else {
    +		      cancelcounter++;
    +		      if (cancelcounter > 5) {
    +	                gotoState=POS_REACHED;
    +	          }
    +		    }
    +
    +			break;
    +
    +		case INITIAL_TURN:
    +		    xDiff=target_x-X_pos;
    +	        yDiff=target_y-Y_pos;
    +		    diffx_last=fabs(xDiff);
    +		    diffy_last=fabs(yDiff);
    +			gotoState=GOTO_LOOP;
    +			cancelcounter_samepos=0;
    +			bot_turn(data,bot_gotoxy_calc_turn(xDiff,yDiff));
    +			break;
    +
    +		case GOTO_LOOP:
    +
    +		     xDiff=target_x-X_pos;
    +	         yDiff=target_y-Y_pos;
    +
    +			/* Position erreicht? */
    +			if (fabs(xDiff)<=1 || fabs(yDiff)<=1 || diffx_last<fabs(xDiff)|| diffy_last<fabs(yDiff)   ) {
    +
    +				gotoState=CORRECT_HEAD;
    +				int16 angle = bot_gotoxy_calc_turn(xDiff,yDiff);
    +				cancelcounter_samepos=0;
    +				bot_turn(data,angle);
    +				diffx_last=99;   // Kennung zum Neulesen wenn Turn-Verhalten zu Ende ist
    +
    +				break;
    +			}
    +
    +
    +			// Check auf Haengenbleiben
    +			if (diffx_last==fabs(xDiff) && diffy_last==fabs(yDiff)) {
    +			  cancelcounter_samepos++;
    +			  if (cancelcounter_samepos>=MAXCOUNT_SAMEPOS) {
    +			    cancelcounter_samepos=0;
    +			    gotoState=INITIAL_GETPATH;
    +			    // etwas rueckwaerts fahren
    +			    bot_drive_distance(data,0,-BOT_SPEED_FOLLOW,10);
    +			    break;
    +			  }
    +			}
    +
    +			if (diffx_last!=fabs(xDiff) || diffy_last!=fabs(yDiff))
    +				 cancelcounter_samepos=0;
    +
    +			 speedWishLeft=BOT_SPEED_MAX;
    +			 speedWishRight=BOT_SPEED_MAX;
    +			 diffx_last=fabs(xDiff);
    +		     diffy_last=fabs(yDiff);
    +
    +			break;
    +
    +		case CORRECT_HEAD:
    +
    +		    xDiff=target_x-X_pos;
    +	        yDiff=target_y-Y_pos;
    +
    +		    if (diffx_last==99) {  // Neulesen nur bei Verhaltenswechsel
    +		      diffx_last=fabs(xDiff);
    +		      diffy_last=fabs(yDiff);
    +		      cancelcounter_samepos=0;
    +		    }
    +
    +
    +			/* Position erreicht? */
    +			 if (map_in_dest (X_pos, Y_pos,target_x, target_y)) {
    +
    +				//Zwischenziel erreicht, check ob dies auch Endziel war
    +				 if (map_in_dest (X_pos, Y_pos,dest_x_map, dest_y_map)) {
    +				    gotoState=POS_REACHED;
    +				    speedWishLeft=BOT_SPEED_STOP;
    +				    speedWishRight=BOT_SPEED_STOP;
    +				    break;
    +				}
    +				else {
    +			 	    // auf jeden Fall bei hoeherer Aufloesung naechstes Zwischenziel
    +					gotoState=NEXT_POS;
    +					break;
    +				}
    +			}  //Ende Endzielauswertung
    +
    +
    +
    +			//Beim erreichen Zeile oder Spalte
    +			if (fabs(xDiff)==0 && fabs(yDiff)==0) {
    +				gotoState=NEXT_POS;
    +				break;
    +			}
    +
    +
    +			if ( diffx_last<fabs(xDiff)|| diffy_last<fabs(yDiff)) {
    +					gotoState=NEXT_POS;
    +				    break;
    +			}
    +
    +			speedWishLeft=BOT_SPEED_NORMAL;
    +			speedWishRight=BOT_SPEED_NORMAL;
    +
    +			if (node_nextdest==NULL)
    +			    gotoState=INITIAL_GETPATH;
    +
    +			// Check auf Haengenbleiben
    +			if (diffx_last==fabs(xDiff) && diffy_last==fabs(yDiff)) {
    +			  cancelcounter_samepos++;
    +			  if (cancelcounter_samepos>=MAXCOUNT_SAMEPOS) {
    +			    cancelcounter_samepos=0;
    +			    gotoState=INITIAL_GETPATH;
    +			    // etwas rueckwaerts fahren
    +			    bot_drive_distance(data,0,-BOT_SPEED_FOLLOW,10);
    +			    break;
    +			  }
    +			}
    +
    +			if (diffx_last!=fabs(xDiff) || diffy_last!=fabs(yDiff))
    +				 cancelcounter_samepos=0;
    +
    +			diffx_last=fabs(xDiff);
    +		    diffy_last=fabs(yDiff);
    +			break;
    +
    +        case NEXT_POS:
    +                    cancelcounter_samepos=0;
    +                    xDiff=target_x-X_pos;
    +	                yDiff=target_y-Y_pos;
    +                    gotoState=INITIAL_TURN;
    +
    +                    // kommt hierher, obwohl die Position des bots nicht uebereinstimmt mit dem eigentlich Soll-Zwischenziel-neue Pfadbestimmung
    +                    if (!map_in_dest (X_pos, Y_pos,(uint16)target_x,(uint16)target_y))
    +                       	node_nextdest=NULL;
    +
    +                    // node ist hier erreicht, nun zum Folgenode gehen wenn vorhanden
    +                    if (node_nextdest!=NULL && node_nextdest->parent_pathnode!=NULL) {
    +
    +                        // naechsten Zielnode bereits setzen
    +                        node_nextdest=node_nextdest->parent_pathnode;
    +
    +                        if (node_nextdest!=NULL) {
    +                          target_x=node_nextdest->xpos;
    +                          target_y=node_nextdest->ypos;
    +                        }
    +                    }
    +                      else
    +                        node_nextdest=NULL;
    +
    +                    if (node_nextdest==NULL)
    +			          gotoState=INITIAL_GETPATH;
    +
    +             break;
    +
    +
    +		case POS_REACHED:
    +			gotoState=INITIAL_GETPATH;
    +			node_nextdest=NULL;  // Variable laut Pfadplaner ist hier die aktuelle Botpos, also ungueltig
    +			cancelcounter=0;
    +			cancelcounter_samepos=0;
    +			cancelcounter_samepos=0;
    +			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; dieses macht bei hoeherer Aufloesung Probleme, da der bot recht nah
    +	 * an die Wand kommt und das Verhalten zuschlaegt und fuer sich das bot_turn-Verhalten exclusiv reserviert
    +	 *
    +	 */
    +	 #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
    +		 #if MAP_RADIUS_FIELDS_GODEST > 1  // nicht bei bot-Aufloesung, nur bei hoeherer Aufloesung
    +				deactivateBehaviour(bot_avoid_col_behaviour);
    +	     #endif
    +	 #endif
    +
    +    gotoState=INITIAL_GETPATH;  //wieder initial setzen, falls mal durch Notstopp undef. war
    +
    +	switch_to_behaviour(caller, bot_gotoxy_behaviour_map, OVERRIDE);  // Umschaltung zum 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 zum Setzen des globalen Zielpunktes; nach Aktivierung wird die Botposition ermittelt und in die
    + *  globale Zielposition gespeichert;das Verhalten wird daraufhin 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
    +              printf("globales Ziel gesetzt auf Botposition: %1d %1d \n",dest_x_map, dest_y_map);
    +            #endif
    +
    +			state++;
    +			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);
    +}
    +
    +
    +
    +
    +/*! ab dem Knoten x,y wird rundum fuer jeden Knoten das Potenzial ermittelt und der Knoten mit dem kleinsten
    + *  Wert bestimmt und zurueckgegeben
    + *  die Nachbarn werden alle in die nodes-Liste eingetragen fuer spaetere weitere Untersuchung
    + *  dies ist die Hauptroutine zur Ermittlung des Potenzials in einem beliebigen Punkt
    + *  @param x, y auf Potenzial zu untersuchende Map-Koordinate
    + *  @param xdest, ydest Zielkoordinaten fuer das attraktive Potenzial
    + *  @param *x_min, *y_min Koordinaten des Nachbar-/Umkreispunktes mit dem kleinsten Potenzial
    + *  @param *parnode Elternnode, also wer war der Vorgaenger, woher komme ich gerade
    + *  @param get_gradient Kennung auf Gradientenabstieg, in Verbindung mit parnode NULL kein Speicheranlegen
    + *  @param XP_START, YP_START  Startkoordinaten des bots
    + */
    +Node_t *calc_min_pot_map_insert(uint16 x, uint16 y, uint16 xdest, uint16 ydest, uint16 *x_min, uint16 *y_min, Node_t *parnode,
    +                                int8 get_gradient, uint16 XP_START, uint16 YP_START) {
    +  int8 i;
    +  int8 j;
    +  float sumpot=0;
    +  float sumpot_att=0;
    +  float sumpot_rep=0;
    +
    +  Node_t *qtemp=NULL;
    +  Node_t *qmin=NULL;
    +
    +  //minimalstes Potenzial wird sich hier gemerkt
    +  float minpot=9999;  //auf maximalen Wert setzen
    +  uint16 x_minpos=0;
    +  uint16 y_minpos=0;
    +  float minpot_rep=0;
    +  float minpot_att=0;
    +
    +  // Rueckgabewerte erst mal init.
    +  *x_min=0;
    +  *y_min=0;
    +
    +  // Umkreisermittlung macht nur Sinn bei hoeherer Aufloesung
    +  #if MAP_RADIUS_FIELDS_GODEST > 1
    +    int8 radius = MAP_RADIUS_FIELDS_GODEST;
    +    uint16 quad=0;     // Quadratwert des Abstandes in der Schleife
    +  #else
    +    int8 radius = 1;  // sonst direkter Nachbar
    +  #endif
    +
    +     //nur innerhalb des Radiuskreises bleiben
    +	 for (i=-radius; i<=radius; i++) {//Zeilenzaehler
    +			for (j=-radius; j<=radius; j++) {//Spaltenzaehler
    +
    +			  #if MAP_RADIUS_FIELDS_GODEST > 1  // Umkreisfelder nur bei hoeherer Aufloesung, sonst 8Nachbarschaft
    +			   if (radius>1)
    +			     quad=i*i + j*j;
    +			   if(radius==1 || quad == MAP_RADIUS_FIELDS_GODEST_QUAD || quad==MAP_RADIUS_FIELDS_GODEST_QUAD-1 || quad==MAP_RADIUS_FIELDS_GODEST_QUAD+1)	{//genau auf Radiusumkreis Feld oder +-1
    +
    +              #endif
    +
    +			  	//Potenzial ermitteln an Indexposition; nicht Mittelpos 0,0 selbst
    +			  	if (((j!=0)||(i!=0)) && x+j>=0 && y+i>=0) {
    +
    +			     // nur innerhalb der Rechteckgrenzen, bei Gradient auch ausserhalb falls Karte nicht aufgebaut
    +			     if (get_gradient || (x+j>=map_xmin && x+j<=map_xmax && y+i>=map_ymin && y+i<map_ymax)) {
    +
    +			      //wenn nicht betretbar laut Map gar nicht erst auswerten und gleich uebergehen
    +			      if (map_get_field(x+j,y+i)>-MAPFIELD_IGNORE ) {
    +
    +			  	    qtemp=node_exists_in_list(&nodes, x+j,y+i);
    +
    +			  	     //darf noch nicht existieren oder nur in Liste A
    +			  	    if (qtemp==NULL || !qtemp->exists_in_queue) {
    +
    +			  	     // Zelle ist Ziel selbst
    +			  	       if (map_in_dest (x+j, y+i,xdest, ydest)) {
    +			  	       sumpot=0;
    +			  	       sumpot_att=0;
    +			  	       sumpot_rep=0;
    +                       }
    +			  	     else {
    +			  	    	// identisch mit bot-Startposition
    +			  	    	if (x+j==XP_START && y+i==YP_START) 	  {
    +  	    	                sumpot_att=0;
    +  	    	                sumpot_rep=1;
    +  	    	                sumpot = 1;
    +			  	    	}
    +			  	    	else {
    +			  	    		  // Zelle ist weder Start noch Ziel, also Potenzial berechnen
    +	                          // hier Wert aus schon gespeichertem Node nehmen
    +	                          sumpot_rep=get_all_rep_pot(x+j,y+i,xdest,ydest);
    +
    +	                          // wenn dieser schon  besucht ist, dann Potenzialverdopplung; damit werden nicht besuchte bevorzugt behandelt
    +	                          if (qtemp!=NULL && qtemp->visited)
    +	                            sumpot_rep = sumpot_rep * 2;
    +
    +	                          if (parnode!=NULL) {  // Vorgaengernode ist vorhanden
    +	                            // dieser Node ist bereits der Parentnode bzw. der parentparent, Potenzial erhoehen, da schon besucht * 4
    +	                            if (parnode->xpos==x+j && parnode->ypos==y+i)
    +	                              sumpot_rep = sumpot_rep * 2; // Abstossungspotenzial verdoppeln
    +	                            if (parnode->parent_pathnode!=NULL && parnode->parent_pathnode->xpos==x+j && parnode->parent_pathnode->ypos==y+i)
    +	                              sumpot_rep = sumpot_rep * 2; // Abstossungspotenzial verdoppeln
    +	                          }  //parnode !=NULL
    +
    +	                          sumpot_att=get_att_pot(x+j,y+i,xdest,ydest);
    +			  	              sumpot=sumpot_att + sumpot_rep;
    +
    +	                      } // war nicht selbst Ziel
    +			  	     } // war nicht in node-Liste existent oder nur in A
    +
    +			  	    // wenn node noch nicht existiert, dann erzeugen; nicht bei reinem Gradientenabstieg
    +	                if (!get_gradient && qtemp==NULL) {
    +			             qtemp=new_node(x+j,y+i,sumpot,False,False,parnode);
    +
    +	                     if (qtemp!=NULL) {
    +	   	                    insert_node_to_list(&nodes, qtemp);
    +	                        if (map_in_dest (x+j, y+i,xdest, ydest))
    +	                           return qtemp;
    +	                     }
    +	                }
    +
    +			  	    //bei kleinerem Potential Werte merken,
    +			  	    if (sumpot<minpot) {
    +			  	  	  //Node darf nicht existieren um Nachbarn einzufuegen, bei Gradabstieg aber auch wenn noch nicht existiert
    +			  	  	  if (qtemp!=NULL || get_gradient) {
    +			  	  	     minpot=sumpot;  // Potenzial nur mit Kosten zum Parent
    +			  	  	     minpot_rep=sumpot_rep;
    +			  	  	     minpot_att=sumpot_att;
    +			  	  	     x_minpos=x+j;
    +			  	  	     y_minpos=y+i;
    +			  	  	     *x_min=x+j;
    +			  	  	     *y_min=y+j;
    +			  	  	     qmin=qtemp;
    +			  	  	  }//if
    +			  	    }//if sumpot<minpot
    +
    +			      }//war schon in Liste B
    +			      }//if Mapfeld nicht betretbar
    +			     } //ausserhalb der Indexgrenzen
    +			  	}//if, Pos selbst
    +             #if MAP_RADIUS_FIELDS_GODEST > 1	  	// nur bei hoeherer Aufloesung
    +			   } //genau auf Radiuskreis
    +			 #endif
    +			  }//for Spalten x->j
    +	 }//for Zeilen y->i
    +
    +	 //hier ist kleinstes Potential ermittelt
    +	 // bei Gradientenabstieg mit uebergebenem Parentnode NULL wird kein Speichereinfuegen des nodes benoetigt
    +	 // dann ist hier Schluss
    +	 if (get_gradient && parnode==NULL)
    +	 	 return NULL;
    +
    +	  // wenn node noch nicht existiert, dann erzeugen fuer Gradabstieg
    +	 if (get_gradient && qmin==NULL && minpot<9999) {
    +
    + 	   // Node mit min. Pot. erzeugen
    + 	   //qmin=new_node(x_minpos,y_minpos,minpot,False,False,parnode);
    + 	   qmin=new_node(x_minpos,y_minpos,minpot,False,False,parnode);
    +
    +	   if (qmin!=NULL) {
    +	   	 // nun einfuegen in nodes-Liste
    +  	     insert_node_to_list(&nodes, qmin);
    +	     if (map_in_dest (x_minpos,y_minpos,xdest, ydest))  //Zielvergleich
    +	         return qmin;
    +
    +	   }  //qmin war != NULL
    +	 }
    +
    +	 // Wert aus Liste A mit geringstem Pot zurueckgeben bei Nachbarbestimmung, also bei Gradientenabstieg
    +	 // wird qmin bereits oben bestimmt, ist dieser Wert aber NULL, dann auch den naechsten der Liste ermitteln
    +	 if (!get_gradient || qmin==NULL) {
    +
    +       qtemp=qmin;
    +	   qmin=get_first_node_from_list(&nodes);  // Rueckgabe des nodes mit geringstem Potenzial
    +
    +	   //falls nix mehr in Liste drin, dann den minimalsten Nachbarn nehmen
    +	   if (qmin==NULL && qtemp!=NULL)
    +	   	qmin=qtemp;
    +	 }
    +
    +	 return qmin;
    +}
    +
    +
    +
    +/*! Generierung des Pfades zum globalen Ziel nach Strategie Best-First und Potenzialfeldmethode
    + *  globales Ziel ist bereits vorher mit anderer Routine (Verhalten) gesetzt
    + *  worden, andernfalls ist das globale Ziel der Startpunkt
    + *  das globale Ziel befindet sich in dest_x_map und dest_y_map
    + * @param start_x_map, start_y_map Anfangskoordinaten der Pfadsuche
    + * @param dest_x_map, dest_y_map Zielkoordinaten der Pfadsuche
    + * @param forward bei True Suche vom Bot-Startpunkt zum echten Endziel, sonst die entgegengesetzten Koordinaten
    + *        fuer Suche vom Endziel zum Startpunkt (normale Suche)
    + */
    +void bot_calc_path_bestfirst(uint16 start_x_map, uint16 start_y_map,
    +                             uint16 dest_x_map, uint16 dest_y_map, int8 forward) {
    +
    +	uint16 x_minpos;
    +	uint16 y_minpos;
    +
    +	uint16 XP;
    +	uint16 YP;
    +
    +	int8 goal_found=False;  // gesetzt bei Ziel gefunden
    +    float minpot=999999;  //auf maximalen Wert setzen
    +
    +	Node_t *q=NULL;
    +	Node_t *qtemp=NULL;
    +
    +	// Wertebereich der Map ermitteln, Werte bei SHRINK_MAP_ONLINE enthalten leider nicht
    +	// das kleinste die Hindernisse umgebende Rechteck
    +    get_rectcoords_environment(&map_xmin,&map_xmax,&map_ymin,&map_ymax);
    +
    +    // globale Zielkoordinaten in lokale Var
    +    uint16 dest_x_map_loc=dest_x_map;
    +	uint16 dest_y_map_loc=dest_y_map;
    +
    +	uint16 XP_START = start_x_map;
    +	uint16 YP_START = start_y_map;
    +
    +   if (map_in_dest(dest_x_map_loc,dest_y_map_loc,XP_START,YP_START))
    +       return;  // Start- ist schon Zielpunkt
    +
    +   if (map_get_field(dest_x_map_loc,dest_y_map_loc)<=-MAPFIELD_IGNORE  )
    +      return;  // Zielfeld ist nicht betretbar
    +
    +  //-------------------------------------------------------------------------
    +  //  1. Startknoten in A einfuegen
    +  //-------------------------------------------------------------------------
    +
    +   // Startnode ist die Botposition-> erst in A eintragen und nicht besucht
    +   q=new_node(XP_START, YP_START,1,False,False,NULL);
    +   // nun einfuegen in nodes-Liste
    +   insert_node_to_list(&nodes, q);
    +
    +  //-------------------------------------------------------------------------
    +  //  2. Solange Liste A nicht leer
    +  //-------------------------------------------------------------------------
    +  int16 counter=0;
    +
    +  while (q!=NULL && counter <= MAXCOUNTER && !goal_found)  {
    +
    +    if (!q->visited) {     // Node wurde noch nicht weiter untersucht
    +    	 q->visited=True;  // als besucht kennzeichnen
    +
    +  	     // Ende wenn Zielkoordinaten im Umkreis oder identisch
    +  	     if (map_in_dest (q->xpos, q->ypos,dest_x_map_loc, dest_y_map_loc)) {
    +		  goal_found=True;
    +		  break;  // while verlassen
    +	     }  // Zielvergleich
    +    }
    +    else {
    +    	  // hier war node bereits als besucht gekennzeichnet und damit schon untersucht worden
    +
    +  //---------------------------------------------------------------------------
    +  //  2.a. move Knoten n (n_act)  aus A nach B (visited setzen)
    +  //---------------------------------------------------------------------------
    +     if (q==NULL)
    +  	   break;
    +
    +    //in die Arbeitsvariablen speichern
    +    XP=q->xpos;
    +    YP=q->ypos;
    +
    +  	q->exists_in_queue = True;    // Kennung setzen fuer Liste B
    +    //-------------------------------------------------------------------------
    +    //  2.b. bearbeite jeden Nachbarn m des Knoten n, ermittelt in 2.a.; insert
    +    //  in A wenn nicht existent; ignorieren wenn laut Map m nicht betretbar
    +    //  wenn m gleich dem globalen Endziel, dann ist Weg gefunden und Schluss
    +    //-------------------------------------------------------------------------
    +
    +       x_minpos=0; y_minpos=0;
    +       minpot=999999;
    +
    + 	   qtemp=q;  // Vorgaengernode merken
    +	   q=NULL;
    +       // alle Nachbarnodes in Liste einfuegen und dann via Pop den mit kleinstem Pot geholt
    +       q=calc_min_pot_map_insert(XP,YP,dest_x_map_loc,dest_y_map_loc,&x_minpos,&y_minpos,qtemp,
    +                                 False,XP_START,YP_START);
    +
    +       if (q==NULL) {
    +   	        counter=9999;
    +  	        break;
    +          }
    +
    +         // Check auf Koordinate im Zielumkreis oder identisch
    +         if (map_in_dest (q->xpos, q->ypos,dest_x_map_loc, dest_y_map_loc)) {
    +	       goal_found=True;
    +	       break;
    +	      }  // Zielvergleich
    +
    +
    +     	  //nach Einfuegen der Nachbarn wurde aus Liste der node mit geringstem Pot geholt; von diesem erfolgt gleich
    +    	  //Gradientenabstieg mit dem naechsten node, so dass dieser hier gekennzeichnet werden muss
    +    	  if (!q->visited)
    +     	    q->visited=True;  // als besucht kennzeichnen
    +
    +	        //die Arbeitsvariablen wieder belegen
    +            XP=q->xpos;
    +            YP=q->ypos;
    +
    +  }   //if von Abfrage auf schon besucht
    +
    +
    +	//nun Gradientenabstieg zum Knoten q, also Nachbar mit kleinstem Pot suchen, der noch nicht in B ist
    +	qtemp=q;
    +	q=NULL;
    +
    +	 q=calc_min_pot_map_insert(qtemp->xpos,qtemp->ypos,dest_x_map_loc,dest_y_map_loc,&x_minpos,&y_minpos,qtemp, True,
    +	                           XP_START,YP_START);
    +	 if (q==NULL)
    +  	     break;
    +
    +	if (map_in_dest (q->xpos, q->ypos,dest_x_map_loc, dest_y_map_loc)) {
    +	       goal_found=True;
    +	       break;
    +	      }  // Zielvergleich
    +
    +	//in die Arbeitsvariablen speichern
    +    XP=q->xpos;
    +    YP=q->ypos;
    +
    +    if (map_in_dest (q->xpos, q->ypos,dest_x_map_loc, dest_y_map_loc)) {
    +	  goal_found=True;
    +	}  // Zielvergleich
    +
    +   // fuer jeden Nachbarn von XP YP
    +	counter++;
    +  } //while nodes <> NULL
    +
    +  if (goal_found)
    +  	//hier jetzt backtracking ab erkanntem Zielnode; Ausgabe der Liste mit den Zielkoordinaten auf PC
    +	show_list_destkoords(&nodes, dest_x_map_loc, dest_y_map_loc, forward);
    +  else
    +    node_nextdest=NULL;
    +
    +
    +}
    +
    +
    +
    +/*! Verhalten fuer Generierung des Pfades vom Bot-Ausgangspunkt zum Ziel nach
    + * Strategie Best-First und Potenzialfeldmethode
    + * @param *data der Verhaltensdatensatz
    + */
    +void bot_path_bestfirst_behaviour(Behaviour_t *data){
    +	#define GET_PATH_BACKWARD 0
    +	#define REACHED_POS       9
    +
    +	static uint8 state=GET_PATH_BACKWARD;
    +
    +	switch (state){
    +		case GET_PATH_BACKWARD:
    +		    // Pfadberechnung rueckwaerts mit global gesetztem Ziel
    +            nodes_free(&nodes);  // Liste und nodes leeren; Speicher freigeben
    +            node_nextdest=NULL;  // naechsten Zielnode init.
    +
    +            // Botposition uebergeben als Startpunkt der Pfadsuche
    +            uint16 X_map_start = world_to_map(x_pos);
    +	        uint16 Y_map_start = world_to_map(y_pos);
    +			bot_calc_path_bestfirst(dest_x_map, dest_y_map, X_map_start, Y_map_start,False);
    +            state = REACHED_POS;
    +
    +			break;
    +
    +
    +		default:
    +			state=GET_PATH_BACKWARD;
    +			return_from_behaviour(data);
    +			break;
    +	}
    +}
    +
    +/*! Botenverhalten zum Aufruf des Pfadberechnungs-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
    +        printf("Botposition ist bereits das Ziel: %2d %2d \n",dest_x_map,dest_y_map);
    +      #endif
    +	}
    +	else  // und Umschaltung zum eigentlichen Verhalten
    +	  switch_to_behaviour(caller, bot_path_bestfirst_behaviour,OVERRIDE);
    +}
    +
    +#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,104 @@
    +/*
    + * 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; Strategie nach Best-First
    + *
    + * @author 	Frank Menzel (Menzelfr@xxxxxxx)
    + * @date 	21.01.07
    +*/
    +
    +#include "bot-logic/bot-logik.h"
    +
    +#ifndef BEHAVIOUR_MAP_GO_DESTINATION_H_
    +#define BEHAVIOUR_MAP_GO_DESTINATION_H_
    +
    +//#ifdef MAP_AVAILABLE
    +
    +#ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    +
    +
    +/*! Verwaltungsstruktur fuer die Nodeliste zur Pfadgenerierung nach Best-First */
    +typedef struct _Node_t {
    +
    +   uint16 xpos;                         /*!< Koordinaten innerhalb der MAP */
    +   uint16 ypos;
    +
    +   float TotalCoast;				    /*!< Gesamtkosten der Potenziale */
    +
    +   struct _Node_t *next;				/*!< Naechster Node in vorwaerts verketteter Liste */
    +
    +   struct _Node_t *parent_pathnode;		/*!< Verweis auf Elternnode laut Pfadplanung; wird rueckwaerts vom Zielnode aufgerollt */
    +
    +   int8 exists_in_queue;				/*!< Kennung, ob Node schon zur B-Liste gehoert oder noch zur Liste A */
    +   int8 visited;                        /*!< Kennung, ob Node besucht wurde oder nicht */
    +
    +	}__attribute__ ((packed)) Node_t;
    +
    +/*! Liste mit allen noch zu untersuchenden Nodes (Liste A) oder bereits endgueltig untersuchten Nodes (Liste B) */
    +extern Node_t *nodes;
    +
    +/*! naechste anzufahrende Position laut Pfadplanung ab Botposition */
    +extern Node_t *node_nextdest;
    +
    +//globale Zielkoordinaten
    +extern volatile uint16 dest_x_map;
    +extern volatile uint16 dest_y_map;
    +
    +
    +/*! Verhalten zur Pfadplanung nach best-first */
    +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);
    +
    +
    +/*! Routine fuer Gradientenabstieg nach der Potenzialfeldmethode */
    +Node_t *calc_min_pot_map_insert(uint16 xmid, uint16 ymid, uint16 xdest, uint16 ydest, uint16 *x_min, uint16 *y_min, Node_t *parnode,
    +                                int8 get_gradient, uint16 XP_START, uint16 YP_START);
    +
    +
    +/*! 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);
    +
    +/*! Bot soll sich zu den relativen Mapkoordinaten vom aktuellen Standpunkt aus bewegen */
    +void bot_gotoxy_map(Behaviour_t *caller, int16 x, int16 y);
    +
    +/*! 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);
    +
    +/*! Hilfsroutine zur Ausgabe der belegten Map, nur sinnvoll bei geringerer Aufloesung */
    +#ifdef PC
    +  void show_labmap(void);
    +#endif
    +
    +#define MAPFIELD_IGNORE          120 /*!< negativer Schwellwert, bei dem Laut Map Hindernis gemeldet wird */
    +
    +
    +#endif /* BEHAVIOUR_MAP_GO_DESTINATION_H_ */
    +#endif /* BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE */

- Antworten:
  - Re: [ct-bot] neues Pfadplanungs-Verhalten nach der erstellten/ online erstellten Karte
    - From: Timo Sandmann
  - [ct-bot] neues Pfadplanungs-Verhalten nach der Map
    - From: Frank Menzel

- Voriger (Datum): Re: [ct-bot] Bootloader für den c't-Bot, Daniel
- Nächster (Datum): Re: [ct-bot] neues Pfadplanungs-Verhalten nach der erstellten/ online erstellten Karte, Timo Sandmann
- Voriger (Thread): [ct-bot] CVS-Update: Bootloader die Zweite, Benjamin Benz
- Nächster (Thread): Re: [ct-bot] neues Pfadplanungs-Verhalten nach der erstellten/ online erstellten Karte, Timo Sandmann
- Indizes:
  - Datum
  - Thread