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