c't Projekte - c't-Bot und c't-Sim -
Mailinglisten
[Voriger (Datum)]
[Nächster (Datum)]
[Voriger (Thread)]
[Nächster (Thread)]
[Nach Datum][Nach Thread]
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=""${ProjDirPath}""/>
> <listOptionValue builtIn="false" value=""${ProjDirPath}/include""/>
> -<listOptionValue builtIn="false" value=""C:\Programme\MinGW\include""/>
> -<listOptionValue builtIn="false" value=""C:\Programme\pthreads\pthreads.2""/>
> +<listOptionValue builtIn="false" value=""C:\MinGW\include""/>
> +<listOptionValue builtIn="false" value=""C:\pthreads\pthreads.2""/>
> </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=""C:\Programme\pthreads\Pre-built.2\lib""/>
> -<listOptionValue builtIn="false" value=""C:\Programme\MinGW\lib""/>
> +<listOptionValue builtIn="false" value=""C:\pthreads\Pre-built.2\lib""/>
> +<listOptionValue builtIn="false" value=""C:\MinGW\lib""/>
> </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