Absender: Frank Menzel
Datum: Mo, 30.04.2007 21:26:10
In-reply-to:
<000001c74c8e$ecf16ae0$fe78a8c0@mexpnew>
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 */