## c't-Projekte - Mailinglisten

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

### [ct-bot] neues Pfadplanungs-Verhalten nach der Map -> ueberarbeitet

Absender: Frank Menzel
Datum: Sa, 19.05.2007 23:15:44
In-reply-to: <4640713C.9080402@xxxxxxxx>

    Hallo,
    habe also alles komplett überarbeitet und entsprechend einsortiert, auch
    die diskutierte Sache mit dem Notfallverhalten, welches dieses
    Pfadfindungsverhalten über einen Abgrund informiert und dann etwas
    rückwärts fährt. Anbei dazu der Patch gegen die devel-Version...
    Mit freundlichen Grüßen
    Frank Menzel

    -----Ursprüngliche Nachricht-----
    Von: ct-bot-entwickler-bounces@xxxxxxxxxxxxxxxxx
    [mailto:ct-bot-entwickler-bounces@xxxxxxxxxxxxxxxxx] Im Auftrag von
    Benjamin Benz
    Gesendet: Dienstag, 8. Mai 2007 14:47
    An: Entwicklung rund um den c't-bot
    Betreff: Re: AW: [ct-bot] neues Pfadplanungs-Verhalten nach der Map

    Hi,
    > wollte mal nachfragen, wie weiterhin mit diesem Verhalten umgegangen
    wird... Hat es vielleicht Chancen in den offiziellen Code ?

    Das Verhalten ist bei Timo und mir auf Begeisterung gestoßen. Ich würde
    mich freuen, wenn wir es in den offiziellen Code integrieren könnten.

    > Muß ich noch irgendwie/ was tun ?
    Ja, ich denke da ist noch ein wenig Feintuning nötig. Eine ganze Reihe
    von Hinweisen kamen ja schon von Timo und mir auf der Mailingliste. Es
    wäre gut, wenn Du die dort angesprochenen Punkte durchgehst. Ich hab
    jetzt nicht mehr ganz auf dem Schirm, was an Details da alles war, aber
    im großen und ganzen stehen an:

    * Einsortierung der Hilfsfunktionen in die passenden Quelldateien
    * Erstellung eines Patches gegen den Devel-Zweig des SVN
    * Entfernung von Debug-Code (#defines, etc)

    Über manches davon können wir auch gerne hier nochmal diskutieren.

    Ein Problem dürfte noch die Code-Größe auf dem MCU und die von Timo
    angedeuteten Page-Faults sein. Siche können wir in den devel-Zweig auch
    eine noch nicht optimierte Version packen, aber sie sollte (ohne
    Änderungen an den Default-Settings) auf den ATmega32 passen. Bevor der
    Code allerdings in den Stable-Zweig kommt, soltlen wir auch die
    Performance-Punkte in den Griff kriegen.

    > Wie ist dieses Verhalten überhaupt von der Idee/Umsetzung her
    angekommen ?
    Wie gesagt, im Prinzip ganz gut. Folgende Dinge würde ich gerne noch
    verbessert sehen:

    * Festhängen in lokalen Minima (dazu gabs ja auch schon ne Diskussion)
    * Verallgemeinerung, so dass man das Verhalten noch etwas besser in
    andere integrieren kann und Hilfsfunktionen nicht mehrfach schreiben
    muss
    * Integration der Kartenausgabe als Greymap

    Insgesamt: Super Sache!

    MfG Benjamin Benz

    Index: C:/botneu/ct-Bot/bot-logic/behaviour_avoid_border.c
    ===================================================================
    --- C:/botneu/ct-Bot/bot-logic/behaviour_avoid_border.c	(revision 1150)
    +++ C:/botneu/ct-Bot/bot-logic/behaviour_avoid_border.c	(working copy)
    @@ -37,5 +37,10 @@

     	if (sensBorderR > BORDER_DANGEROUS)
     		speedWishRight=-BOT_SPEED_NORMAL;
    +
    +	 // Start der registrierten Notfall-Routinen zum informieren der Verhalten
    +	if (sensBorderL > BORDER_DANGEROUS || sensBorderR > BORDER_DANGEROUS)
    +	   start_registered_emergency_procs();
    +
     }
     #endif
    Index: C:/botneu/ct-Bot/bot-logic/behaviour_map_go_destination.c
    ===================================================================
    --- C:/botneu/ct-Bot/bot-logic/behaviour_map_go_destination.c	(revision 0)
    +++ C:/botneu/ct-Bot/bot-logic/behaviour_map_go_destination.c	(revision 0)
    @@ -0,0 +1,1056 @@
    +/*
    + * 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 vorherige Pfadplanung nach der Potenzialfeldmethode
    + *          vorhandene Hindernisse; die Zielkoordinate ist zu Beginn die
    + *          Startposition und kann mittels Tastendruck(3) 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"
    +
    +#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;
    +
    +// 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
    +
    +#define MAXCOUNTER 100           // Zielsuche bis zu dieser Tiefe, hier wird Routine spaetestens beendet
    +
    +/*! Werte, wie bestimmte Mapfelder behandelt werden
    + */
    + #define PATHNODES  127      // aus Pfadplanung
    + #define GOAL  127      // Ziel bekommt freieste Feldwahrscheinlichkeit
    +
    +
    +/*! globale Zielkoordinaten fuer Pfadplanung; ist zuerst immer das Bot-Map-Startfeld, da dieses sonst nirgendwo anders
    + *  gespeichert ist; kann sonst via bot_set_destination auf beliebige Map-Pos gesetzt werden
    + */
    +uint16 dest_x_map=0;
    +uint16 dest_y_map=0;
    +
    +/*! 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
    +
    +#ifdef MEASURE_MOUSE_AVAILABLE
    +  static uint8 statehangon=0;  // Statusvar des Haengon-Verhaltens
    +#endif
    +
    +/*! 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; ueber ueber registrierter Prozedur gesetzt vom Abgrundverhalten
    +*/
    +uint8 border_fired=False;
    +
    +
    +/*! True, wenn vom Haengenbleiben-Verhalten das Haengenbleiben des bots erkannt wurde
    + *  muss von Anwendung nach Auswertung wieder auf False gesetzt werden
    + */
    +uint8 hangon_behaviour_fired=False;
    +
    +
    + /*! Elternposition, d.h. Botposition vor neuer Position
    + */
    +static uint16 x_parent=0;
    +static uint16 y_parent=0;
    +
    +
    +// Notfallhandler, ausgefuehrt bei Abgrunderkennung; muss registriert werden
    +void border_mapgo_handler(void){
    +
    +  // Routine muss zuerst checken, ob map_go_destination auch gerade aktiv ist, da nur in diesem
    +  // Fall etwas gemacht werden muss
    +  if (!Behaviour_is_activated(bot_gotoxy_behaviour_map))
    +      return;
    +
    +    border_fired=True;  // Setzen der Syncvar des Verhaltens, die immer abgefragt wird
    +
    + 	bot_drive_distance(0,0,-BOT_SPEED_FOLLOW,10);// 10cm rueckwaerts nur bei Hindernis
    +
    +}
    +
    +
    +// ============= eigenstaendiges Parallelverhalten zum eigentlichen Fahrverhalten =======
    +
    +
    +/*! Verhalten zum Erkennen des Haengenbleibens des Bots; wenn MEASURE_MOUSE_AVAILABLE gesetzt werden Maus- und
    + *  Encoderdaten miteinander verglichen; 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
    + */
    +#ifdef MEASURE_MOUSE_AVAILABLE
    +void bot_check_hang_on_behaviour(Behaviour_t *data){
    +
    +	switch (statehangon){
    +		case 0:
    +		       statehangon++;
    +		       break;
    +		case 1:
    +
    +		  // Check auf Haengenbleiben mit Rad-Durchdreh-Erkennung mit Mausvergleich
    +             // 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)
    +                  ))
    +             {
    +
    +			    // 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;
    +			  }
    +			break;
    +		default:
    +		    // wird eigentlich nicht erreicht, da Endlosverhalten
    +			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;
    +	switch_to_behaviour(caller, bot_check_hang_on_behaviour,NOOVERRIDE);
    +}
    +
    +#endif
    +
    +
    +/*! Routine zum Setzen der Zielkoordinaten; wird 0 uebergeben, dann auf aktuelle Botposition setzen
    + *  @param x X-Map-Zielkoordinate
    + *  @param y Y-Map-Zielkoordinate
    + */
    +void bot_set_destination(uint16 x, uint16 y) {
    +
    +	if (x==0 && y==0) {
    +	  dest_x_map=world_to_map(x_pos);
    +	  dest_y_map=world_to_map(y_pos);
    +	}
    +	else {
    +    	dest_x_map = x;
    +	    dest_y_map = y;
    +	}
    +}
    +
    +
    +
    +
    +// ===== 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;
    +}
    +
    +
    +
    +
    +/*! 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 (map_in_dest(xs,ys,x_parent,y_parent)/*||map_in_dest(xs,ys,x_parent_par,y_parent_par)*/)/*(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)) {
    +		int16 avg=map_get_average_fields(xs,ys,MAP_RADIUS_FIELDS_GODEST_HALF);
    +		sumpot = get_rep_pot(xs,ys,avg);
    +	  }
    +
    +	sumpot+=pot_off_to_parent(xs,ys,sumpot);  // Potenzial verdoppelt falls Elternnode derjenige ist
    +
    +    return sumpot;  // Rueckgabe des repulsiven Summenpotenzials
    +}
    +
    +
    +
    +/*! 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 && !map_in_dest(xj,yi,x_parent,y_parent) /*&& !map_in_dest(xj,yi,x_parent_par,y_parent_par)*//*(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
    +	                       sumpotrep=0;
    +	                       //koennte auch mehrere geben, daher den mit geringstem pot
    +	                       if (map_in_dest(xj,yi,dest_x_map,dest_y_map)) {
    +	                       	sumpot = 0;
    +	                       }
    +	                       else {
    +	                       	 if (get_next_pathpoint) {
    +	                       	   sumpotrep=get_all_rep_pot(xj,yi,xdest,ydest);
    +	                       	   sumpot=sumpotrep + get_att_pot(xj,yi,xdest,ydest);
    +	                       	 }
    +	                       	 else {
    +	                           sumpotrep=get_all_rep_pot(xj,yi,xdest,ydest);
    +	                           sumpot=sumpotrep + get_att_pot(xj,yi,xdest,ydest);
    +	                       	 }
    +	                       }
    +			  	    //bei kleinerem Potenzial Werte merken, Abstossungspot muss auch > Ignore-Potenzial sein
    +			  	    if (sumpot<minpot && (get_next_pathpoint||(sumpotrep < 255 && !get_next_pathpoint))) {
    +			  	  	  //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;
    +
    +	      // wenn Weg zum Ziel nicht frei ist oder Abgrund um Bot, dann gleich mit Pfadplaung weiter
    +	      if (value_in_circle(X_pos,Y_pos,MAP_RADIUS_FIELDS_GODEST_HALF,-128)) {
    +	      	laststate = CLEAR_MAP;  // nicht mehr direction_goal
    +		  	gotoStatexy = CLEAR_MAP;
    +	      }
    +
    +		  break;
    +
    +		case CLEAR_MAP:
    +		   // MAP von altem Pfad und kuenstlichen Hindernissen befreien
    +		   clear_map(-MAP_ART_HAZARD, -MAPFIELD_IGNORE);
    +		   gotoStatexy=CLEAR_MAP_FREEFIELDS;
    +		   break;
    +
    +		case CLEAR_MAP_FREEFIELDS:
    +		   // MAP nun von Freifeldern befreien-Originalmap wieder da ohne Freiwahrscheinlichkeiten voraus
    +		   clear_map(-HAZPOT,127);
    +		   gotoStatexy=INITIAL_GETPATH;
    +
    +		   break;
    +
    +		case INITIAL_GETPATH:
    +		  #ifdef MEASURE_MOUSE_AVAILABLE
    +			  deactivateBehaviour(bot_check_hang_on_behaviour);
    +		  #endif
    +		    // 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
    +	        #ifdef MEASURE_MOUSE_AVAILABLE
    +	          bot_check_hang_on(0);
    +			#endif
    +			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
    +			// vom Notfallverhalten wurde durch die registrierte Proc Abgrundkennung gesetzt
    +			 if  (border_fired) {
    +               border_fired=False;  // wieder ruecksetzen, erst durch Abgrundverhalten neu gesetzt
    +			   gotoStatexy=NEXT_IS_HAZARD;	  // Hindernis voraus
    +			   break;
    +             }
    +
    +			//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; koennte genauso behandelt werden
    +           // wie beim Abgrundverhalten mit Registrierung; wird aber hier immer ab- und zugeschaltet
    +		   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);  // kuenstl. Hindernis
    +
    +           }
    +           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);
    +	       float alpha = (heading * M_PI /180);
    +	       x_nexthaz=get_mapposx_dist(x_pos,y_pos,alpha,100);
    +	       y_nexthaz=get_mapposy_dist(x_pos,y_pos,alpha,100);
    +
    +	       //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);
    +	           x_nexthaz=get_mapposx_dist(x_pos,y_pos,alpha,150);
    +	           y_nexthaz=get_mapposy_dist(x_pos,y_pos,alpha,150);
    +	       }
    +
    +		   // neue Pfadplanung wird erforderlich
    +		   gotoStatexy=INITIAL_GETPATH;
    +
    +		   // Hohe Hinderniswahrscheinlichkeit mit Umfeldaktualisierung der -50 in Map eintragen
    +		   map_set_value_occupied(x_nexthaz,y_nexthaz,-HAZPOT);
    +
    +  	       // ist naechstes Ziel nicht Endziel, dann Richtung Hindernis mit Umfeld druecken
    +  	         if (!map_in_dest(target_x,target_y,dest_x_map,dest_y_map))
    +			       map_set_value_occupied(target_x,target_y,-HAZPOT);
    +
    +		   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 (value_in_circle(X_pos,Y_pos,MAP_RADIUS_FIELDS_GODEST_HALF,PATHNODES))
    +				    	  map_set_value_occupied (target_x,target_y,-HAZPOT);
    +
    +                      // 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, target_x,target_y,&next_x,&next_y,True, MAP_RADIUS_FIELDS_GODEST);
    +                         if (next_x==0 && next_y==0)
    +                         	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 freier Fahrt zum Endepunkt Zielfahrt
    +		            if (map_way_free_fields(X_pos,Y_pos,dest_x_map,dest_y_map)) {
    +		              gotoStatexy=DIRECTION_GOAL;
    +		              break;
    +		            }
    +
    +                    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
    +			#ifdef MEASURE_MOUSE_AVAILABLE
    +			  deactivateBehaviour(bot_check_hang_on_behaviour);
    +			#endif
    +			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, uint16 x, uint16 y){
    +	//Mauskoords in  Mapkoordinaten
    +	uint16 X_pos = world_to_map(x_pos);
    +	uint16 Y_pos = world_to_map(y_pos);
    +
    +	//globale Zielkoordinaten setzen, die fuer Pfadplaner Verwendung finden
    +	if (x!=0 || y!=0) {       // fuer Fahrt relativ zu der Botposition
    +	  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
    +				deactivateBehaviour(bot_avoid_col_behaviour);
    +	 #endif
    +
    +
    +   /* Einschalten sicherheitshalber des on_the_fly Verhaltens, wird bei Notaus sonst deaktiviert
    +    */
    +    #ifdef BEHAVIOUR_SCAN_AVAILABLE
    +		activateBehaviour(bot_scan_onthefly_behaviour);
    +	#endif
    +	/* Einschalten sicherheitshalber des Abgrund-Verhaltens
    +    */
    +    #ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE
    +		activateBehaviour(bot_avoid_border_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;
    +    static uint8 counter_noway=0;
    +
    +	switch (state){
    +		case GET_PATH_INITIAL:
    +
    +			// alle als frei markierten Mappositionen > 0 wieder auf 0 setzen
    +	       clear_map(0,127);  // 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,MAP_RADIUS_FIELDS_GODEST_HALF,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_update_occupied(next_x,next_y);           // Richtung Hindernis druecken mit Umfeld
    +                  map_set_field(next_x,next_y,-MAP_ART_HAZARD); // lok. Min. direkt auf Punkt unpassierbar
    +
    +                 map_update_occupied(X,Y);  //aktuelle Pos unwahrscheinlicher
    +
    +                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;
    +			// zuerst mehrere Versuche starten fuer Zielfindung, falls dieses nicht ermittelt
    +			// werden konnte
    +			counter_noway++;
    +			if (next_x==0 && next_y==0 && (counter_noway < 4)) {
    +				if (counter_noway==1) {
    +				  state=GET_PATH_INITIAL;
    +				  pathcounter=0;
    +				  clear_map(-MAP_ART_HAZARD, -MAP_ART_HAZARD+10);  // Loeschen der kuenstlichen Hindernisse
    +				  break;
    +			    }
    +			    if (counter_noway==2) {
    +				  state=GET_PATH_INITIAL;
    +				  pathcounter=0;
    +				  clear_map(-MAP_ART_HAZARD, 127);  // Loeschen der kuenstlichen Hindernisse
    +				  break;
    +			    }
    +			   if (counter_noway==3) {
    +				  state=DIRECTION_GOAL;
    +				  break;
    +			    }
    +
    +			}
    +			counter_noway=0;
    +            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)) {
    +	  // 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);
    +	}
    +}
    +
    +
    +
    +
    +
    +#ifdef DISPLAY_AVAILABLE
    +	#ifdef DISPLAY_MAP_GO_DESTINATION
    +
    +	static void mapgo_disp_key_handler(void){
    +		/* Keyhandling um Map-Verhalten zu starten*/
    +		switch (RC5_Code){
    +			#ifdef PC
    +			case RC5_CODE_1:
    +			       RC5_Code = 0;
    +			       print_map();
    +			       break;
    +			#endif
    +			case RC5_CODE_3:
    +			       RC5_Code = 0;
    +			       bot_set_destination(world_to_map(x_pos),world_to_map(y_pos));
    +			       break;
    +			 case RC5_CODE_4:
    +			       RC5_Code = 0;
    +			       clear_map(-MAP_ART_HAZARD, -MAPFIELD_IGNORE);  // Loeschen der kuenstlichen Hindernisse und des alten Pfades fuer Neubeginn
    +			       next_x=0;
    +			       next_y=0;
    +			       bot_gotodest_map(0);
    +			      break;
    +			  case RC5_CODE_7:
    +			       // Volldrehung mit Mapaktualisierung
    +			       RC5_Code = 0;
    +                   bot_scan(0);
    +			       break;
    +		     }//case
    +
    +	}  // Ende Keyhandler
    +
    +
    +		/*!
    +		 * @brief	Display zum Start der Map-Routinen
    +		 */
    +		void mapgo_display(void){
    +
    +		  display_cursor(1,1);
    +		  display_printf("MAP last %1d %1d",dest_x_map,dest_y_map);
    +		  display_cursor(2,6);
    +		  display_printf(     "Bot %1d %1d",world_to_map(x_pos),world_to_map(y_pos));
    +		  display_cursor(3,1);
    +		  display_printf("Pos Save/Goto: 3/4");
    +		 // display_cursor(3,1);
    +		  //display_printf("fahre zu Pos : 4 ");
    +		  #ifdef PC
    +		    display_cursor(4,4);
    +		    display_printf("Print/Scan: 1/7");
    +		  #else
    +		    display_cursor(4,10);
    +		    display_printf(         "Scan: 7");
    +          #endif
    +		  mapgo_disp_key_handler();		  // registrieren des Key-Handlers
    +
    +		}
    +	#endif	// MAPGO_DISPLAY
    +#endif
    +
    +#endif  // BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    Index: C:/botneu/ct-Bot/bot-logic/bot-logik.c
    ===================================================================
    --- C:/botneu/ct-Bot/bot-logic/bot-logik.c	(revision 1150)
    +++ C:/botneu/ct-Bot/bot-logic/bot-logik.c	(working copy)
    @@ -71,6 +71,35 @@
     #endif

    +#define MAX_PROCS 3         /*!< Maximale Anzahl der registrierbaren Funktionen */
    +int8 count_arr_emerg = 0;	/*!< Anzahl der zurzeit registrierten Notfallfunktionen */
    +/*!< hier liegen die Zeiger auf die auszufuehrenden Abgrund Notfall-Funktionen */
    +static void (* emerg_functions[MAX_PROCS])(void) = {NULL};
    +
    +/*! Routine zum Registrieren einer Notfallfunktion, die beim Ausloesen eines Abgrundsensors
    + *  aufgerufen wird; hierdurch kann ein Verhalten vom Abgrund benachrichtigt werden und
    + *  entsprechend dem Verhalten reagieren
    + * @param fkt die zu registrierende Routine, welche aufzurufen ist
    + * @return Index, den die Routine im Array einnimmt, bei -1 ist alles voll
    + */
    +int8 register_emergency_proc(void* fkt){
    +	if (count_arr_emerg == MAX_PROCS) return -1;	// sorry, aber fuer dich ist kein Platz mehr da :(
    +	int8 proc_nr = count_arr_emerg++;		// neue Routine hinten anfuegen
    +	emerg_functions[proc_nr] = fkt;	// Pointer im Array speichern
    +	return proc_nr;
    +}
    +
    +/*! Beim Ausloesen eines Abgrundes wird diese Routine am Ende des Notfall-Abgrundverhaltens angesprungen
    + *  und ruft alle registrierten Prozeduren der Reihe nach auf
    + */
    +void start_registered_emergency_procs(void) {
    + uint8 i=0;
    +	for (i=0; i<MAX_PROCS; i++){
    +		if (emerg_functions[i] != NULL)
    +		    emerg_functions[i]();
    +	}
    +}
    +
     /*!
      * Das einfachste Grundverhalten
      * @param *data der Verhaltensdatensatz
    @@ -116,10 +145,14 @@

     	#ifdef BEHAVIOUR_SCAN_AVAILABLE
     		// Verhalten, das die Umgebung des Bots on-the fly beim fahren scannt
    -		insert_behaviour_to_list(&behaviour, new_behaviour(155, bot_scan_onthefly_behaviour,ACTIVE));
    -
    -		// Verhalten, das einmal die Umgebung des Bots scannt
    -		insert_behaviour_to_list(&behaviour, new_behaviour(152, bot_scan_behaviour,INACTIVE));
    +		insert_behaviour_to_list(&behaviour, new_behaviour(202, bot_scan_onthefly_behaviour,ACTIVE));
    +        // vom Notfallverhalten wird Position des Abgrundes in Map eingetragen durch
    +		// Aufruf dieser registrierten Proc
    +		#ifdef MAP_AVAILABLE
    +		  register_emergency_proc(&border_in_map_handler);
    +		#endif
    +		  // Verhalten, das einmal die Umgebung des Bots scannt
    +		  insert_behaviour_to_list(&behaviour, new_behaviour(152, bot_scan_behaviour,INACTIVE));
     	#endif

     	// Alle Hilfsroutinen sind relativ wichtig, da sie auch von den Notverhalten her genutzt werden
    @@ -142,6 +175,20 @@
     	#ifdef BEHAVIOUR_MEASURE_DISTANCE_AVAILABLE
     		insert_behaviour_to_list(&behaviour, new_behaviour(145, bot_measure_distance_behaviour, INACTIVE));
     	#endif
    +
    +	// Verhalten, um laut Map zu einem bestimmten Ziel zu fahren
    +    #ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    +        //insert_behaviour_to_list(&behaviour, new_behaviour(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));
    +	    #ifdef MEASURE_MOUSE_AVAILABLE
    +	      insert_behaviour_to_list(&behaviour, new_behaviour(137, bot_check_hang_on_behaviour,INACTIVE));
    + 	    #endif
    + 	    insert_behaviour_to_list(&behaviour, new_behaviour(135, bot_gotoxy_behaviour_map,INACTIVE));
    + 	    bot_set_destination(0,0);  // auf aktuelle Botposition setzen (bei 0,0 sonst Mappos selbst)
    + 	    // Registrierung zur Behandlung des Notfallverhaltens zum Rueckwaertsfahren
    + 	    register_emergency_proc(&border_mapgo_handler);
    +    #endif

     	#ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE
     		insert_behaviour_to_list(&behaviour, new_behaviour(44, bot_catch_pillar_behaviour,INACTIVE));
    @@ -220,6 +267,23 @@
     }

     /*!
    + * Rueckgabe von True, wenn das Verhalten gerade laeuft (aktiv ist) sonst False
    + * @param function Die Funktion, die das Verhalten realisiert.
    + * @return True wenn Verhalten aktiv sonst False
    + */
    +uint8 Behaviour_is_activated(BehaviourFunc function){
    +	Behaviour_t *job;						// Zeiger auf ein Verhalten
    +
    +	// Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben
    +	for (job = behaviour; job; job = job->next) {
    +		if (job->work == function)
    +			return job->active;
    +	}
    +	return False;
    +}
    +
    +
    +/*!
      * liefert 1 zurueck, wenn function ueber eine beliebige Kette (job->caller->caller ....) von anderen Verhalten job aufgerufen hat
      * @param job Zeiger auf den Datensatz des aufgerufenen Verhaltens
      * @param function Das Verhalten, das urspruenglich aufgerufen hat
    Index: C:/botneu/ct-Bot/bot-logic/behaviour_scan.c
    ===================================================================
    --- C:/botneu/ct-Bot/bot-logic/behaviour_scan.c	(revision 1150)
    +++ C:/botneu/ct-Bot/bot-logic/behaviour_scan.c	(working copy)
    @@ -40,6 +40,8 @@

     	#ifdef MAP_AVAILABLE
    +
    +
     		static float last_x, last_y, last_head;

     		float diff_x = fabs(x_pos-last_x);
    @@ -69,7 +71,7 @@
     				print_map();
     			}
     		*/
    -	#endif
    +	#endif  //MAP_AVAILABLE
     }

    @@ -141,4 +143,48 @@
     //	update_map(x_pos,y_pos,heading,sensDistL,sensDistR);
     //	print_map();
     }
    +
    +#ifdef MAP_AVAILABLE
    +	/*! 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
    +   // uint8 border_behaviour_fired=False;
    +
    +    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
    +	}
    +
    +}
    +
    +/*!
    + *  Notfallhandler, ausgefuehrt bei Abgrunderkennung; muss registriert werden um
    + *  den erkannten Abgrund in die Map einzutragen
    + */
    +void border_in_map_handler(void){
    +
    +  // Routine muss zuerst checken, ob on_the_fly auch gerade aktiv ist, da nur in diesem
    +  // Fall etwas gemacht werden muss
    +  if (!Behaviour_is_activated(bot_scan_onthefly_behaviour))
    +      return;
    +
    +   /* bei Abgrunderkennung Position des Abgrundes in Map mit -128 eintragen */
    +   update_map_hole(x_pos,y_pos,heading);
    +}
     #endif
    +
    +#endif
    Index: C:/botneu/ct-Bot/bot-logic/behaviour_gotoxy.c
    ===================================================================
    --- C:/botneu/ct-Bot/bot-logic/behaviour_gotoxy.c	(revision 1150)
    +++ C:/botneu/ct-Bot/bot-logic/behaviour_gotoxy.c	(working copy)
    @@ -25,20 +25,7 @@
     */

     #include "bot-logic/bot-logik.h"
    -
    -#ifdef BEHAVIOUR_GOTOXY_AVAILABLE
     #include <math.h>
    -
    -
    -
    -
    -/* Parameter fuer bot_gotoxy_behaviour-Verhalten */
    -float target_x;				/*!< Zielkoordinate X */
    -float target_y;				/*!< Zielkoordinate Y */
    -float initialDiffX;			/*!< Anfangsdifferenz in X-Richtung */
    -float initialDiffY;			/*!< Anfangsdifferenz in Y-Richtung */
    -
    -
     /*!
      * Auslagerung der Berechnung der benoetigten Drehung aus dem gotoxy_behaviour
      * @param xDiff
    @@ -64,6 +51,18 @@
     	return toTurn;
     }

    +#ifdef BEHAVIOUR_GOTOXY_AVAILABLE
    +
    +
    +/* Parameter fuer bot_gotoxy_behaviour-Verhalten */
    +float target_x;				/*!< Zielkoordinate X */
    +float target_y;				/*!< Zielkoordinate Y */
    +float initialDiffX;			/*!< Anfangsdifferenz in X-Richtung */
    +float initialDiffY;			/*!< Anfangsdifferenz in Y-Richtung */
    +
    +
    +
    +
     /*!
      * Das Verhalten faehrt von der aktuellen Position zur angegebenen Position (x/y)
      * @param *data der Verhaltensdatensatz
    Index: C:/botneu/ct-Bot/include/bot-logic/behaviour_gotoxy.h
    ===================================================================
    --- C:/botneu/ct-Bot/include/bot-logic/behaviour_gotoxy.h	(revision 1150)
    +++ C:/botneu/ct-Bot/include/bot-logic/behaviour_gotoxy.h	(working copy)
    @@ -29,6 +29,12 @@
     #define BEHAVIOUR_GOTOXY_H_

     #include "bot-logic/bot-logik.h"
    +/*!
    + * Auslagerung der Berechnung der benoetigten Drehung aus dem gotoxy_behaviour
    + * @param xDiff
    + * @param yDiff
    + */
    +int16 bot_gotoxy_calc_turn(float xDiff, float yDiff);

     #ifdef BEHAVIOUR_GOTOXY_AVAILABLE
     /*!
    @@ -45,6 +51,8 @@
      * @param y Y-Ordinate an die der Bot fahren soll
      */
     void bot_gotoxy(Behaviour_t *caller, float x, float y);
    +
    +
     #endif

     #endif /*BEHAVIOUR_GOTOXY_H_*/
    Index: C:/botneu/ct-Bot/include/bot-logic/behaviour_map_go_destination.h
    ===================================================================
    --- C:/botneu/ct-Bot/include/bot-logic/behaviour_map_go_destination.h	(revision 0)
    +++ C:/botneu/ct-Bot/include/bot-logic/behaviour_map_go_destination.h	(revision 0)
    @@ -0,0 +1,91 @@
    +/*
    + * 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 uint16 dest_x_map;
    +extern uint16 dest_y_map;
    +
    +
    +/*! 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
    +*/
    +extern uint8 border_behaviour_fired;
    +
    +
    +/*! 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;
    +
    +
    +/*! 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);
    +void bot_set_destination(uint16 x, uint16 y) ;
    +
    +/*! 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);
    +
    +int8 register_emergency_proc(void* fkt);
    +void border_mapgo_handler(void);
    +
    +
    +#endif /* BEHAVIOUR_MAP_GO_DESTINATION_H_ */
    +#endif /* BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE */
    Index: C:/botneu/ct-Bot/include/bot-logic/available_behaviours.h
    ===================================================================
    --- C:/botneu/ct-Bot/include/bot-logic/available_behaviours.h	(revision 1150)
    +++ C:/botneu/ct-Bot/include/bot-logic/available_behaviours.h	(working copy)
    @@ -9,7 +9,7 @@
     #define BEHAVIOUR_AVOID_BORDER_AVAILABLE	/*!< Abgruenden ausweichen vorhanden ?*/
     #define BEHAVIOUR_AVOID_COL_AVAILABLE	/*!< Hindernis ausweichen vorhanden ?*/
     //#define BEHAVIOUR_GOTO_AVAILABLE	/*!< goto vorhanden ?*/
    -//#define BEHAVIOUR_GOTOXY_AVAILABLE	/*!< gotoxy vorhanden ?*/
    +#define BEHAVIOUR_GOTOXY_AVAILABLE	/*!< gotoxy vorhanden ?*/
     #define BEHAVIOUR_TURN_AVAILABLE	/*!< turn vorhanden ?*/

     #define BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE	/*!< strecke fahren vorhanden ?*/
    @@ -19,6 +19,7 @@
     #define BEHAVIOUR_SCAN_AVAILABLE	/*!< gegend scannen vorhanden ?*/
     #define BEHAVIOUR_SOLVE_MAZE_AVAILABLE	/*!< Wandfolger vorhanden ?*/
     //#define BEHAVIOUR_FOLLOW_LINE_AVAILABLE	/*!< Linienfolger vorhanden ?*/
    +#define BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE /*!< Fahren zu einem Ziel nach Pfadplanung*/

     #define BEHAVIOUR_SERVO_AVAILABLE 	/*!< Kontrollverhalten fuer die Servos */

    @@ -37,6 +38,7 @@
      */
     #ifndef MAP_AVAILABLE
     	#undef BEHAVIOUR_SCAN_AVAILABLE
    +	#undef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
     #endif

     #ifdef BEHAVIOUR_GOTOXY_AVAILABLE
    @@ -100,6 +102,7 @@

     #include "bot-logic/behaviour_scan.h"

    +#include "bot-logic/behaviour_map_go_destination.h"

     #include "bot-logic/behaviour_solve_maze.h"
     #include "bot-logic/behaviour_follow_line.h"
    Index: C:/botneu/ct-Bot/include/bot-logic/bot-logik.h
    ===================================================================
    --- C:/botneu/ct-Bot/include/bot-logic/bot-logik.h	(revision 1150)
    +++ C:/botneu/ct-Bot/include/bot-logic/bot-logik.h	(working copy)
    @@ -116,6 +116,13 @@
     void deactivateBehaviour(BehaviourFunc function);

     /*!
    + * Rueckgabe von True, wenn das Verhalten gerade laeuft (aktiv ist) sonst False
    + * @param function Die Funktion, die das Verhalten realisiert.
    + * @return True wenn Verhalten aktiv sonst False
    + */
    +uint8 Behaviour_is_activated(BehaviourFunc function);
    +
    +/*!
      * Deaktiviert alle Verhalten bis auf Grundverhalten. Bei Verhaltensauswahl werden die Aktivitaeten vorher
      * in die Verhaltens-Auswahlvariable gesichert.
      */
    @@ -158,6 +165,19 @@
      */
     void insert_behaviour_to_list(Behaviour_t **list, Behaviour_t *behave);

    +/*! Routine zum Registrieren einer Notfallfunktion, die beim Ausloesen eines Abgrundsensors
    + *  aufgerufen wird; hierdurch kann ein Verhalten vom Abgrund benachrichtigt werden und
    + *  entsprechend dem Verhalten reagieren
    + * @param fkt die zu registrierende Routine, welche aufzurufen ist
    + * @return Index, den die Routine im Array einnimmt, bei -1 ist alles voll
    + */
    +int8 register_emergency_proc(void* fkt);
    +
    +/*! Beim Ausloesen eines Abgrundes wird diese Routine am Ende des Notfall-Abgrundverhaltens angesprungen
    + *  und ruft alle registrierten Prozeduren der Reihe nach auf
    + */
    +void start_registered_emergency_procs(void);
    +
     /*!
      * @brief			Erzeugt ein neues Verhalten
      * @param priority 	Die Prioritaet
    Index: C:/botneu/ct-Bot/include/bot-logic/behaviour_scan.h
    ===================================================================
    --- C:/botneu/ct-Bot/include/bot-logic/behaviour_scan.h	(revision 1150)
    +++ C:/botneu/ct-Bot/include/bot-logic/behaviour_scan.h	(working copy)
    @@ -42,6 +42,13 @@
      */
     void bot_scan_behaviour(Behaviour_t *data);

    +
    +/*!
    + *  Notfallhandler, ausgefuehrt bei Abgrunderkennung; muss registriert werden um
    + *  den erkannten Abgrund in die Map einzutragen
    + */
    +void border_in_map_handler(void);
    +
     /*!
      * Der Roboter faehrt einen Vollkreis und scannt dabei die Umgebung
      * @param Der aufrufer
    Index: C:/botneu/ct-Bot/include/bot-local.h
    ===================================================================
    --- C:/botneu/ct-Bot/include/bot-local.h	(revision 1150)
    +++ C:/botneu/ct-Bot/include/bot-local.h	(working copy)
    @@ -37,6 +37,7 @@

     #define DISTSENSOR_POS_FW	35	/*!< Abstand der Distanzsensoren von der Radachse (in fahrtrichtung)*/
     #define DISTSENSOR_POS_SW	32	/*!< Abstand der Distanzsensoren von der Mittelachse (in querrichtung)*/
    +#define DISTSENSOR_POS_B_SW   (DISTSENSOR_POS_SW + 5) /*!< Abgrundsensoren 5mm weiter aussen als Distsensoren*/

     /* Parameter der Motorregelung */
     #define PID_Kp				70				/*!< PID-Parameter proportional */
    @@ -105,12 +106,12 @@

     /*! Konstante fuer das bot_follow_line_behaviour-Verhalten */
    -#define LINE_SENSE					0x350	// Ab wann ist es Linie? (Fuer den Sim auf 350 setzen, helle Tischflaeche 50)
    +#define LINE_SENSE					0x290	// Ab wann ist es Linie? (Fuer den Sim auf 350 setzen, helle Tischflaeche 50)

     /* Konstanten fuer Verhaltensanzeige, Verhalten mit prio von bis sichtbar */
     #define PRIO_VISIBLE_MIN 3			/*!< Prioritaet, die ein Verhalten mindestens haben muss, um angezeigt zu werden */
    -#define PRIO_VISIBLE_MAX 154		/*!< Prioritaet, die ein Verhalten hoechstens haben darf, um angezeigt zu werden */
    +#define PRIO_VISIBLE_MAX 199		/*!< Prioritaet, die ein Verhalten hoechstens haben darf, um angezeigt zu werden */

    Index: C:/botneu/ct-Bot/include/map.h
    ===================================================================
    --- C:/botneu/ct-Bot/include/map.h	(revision 1150)
    +++ C:/botneu/ct-Bot/include/map.h	(working copy)
    @@ -51,6 +51,21 @@
     	#define MAP_SECTION_POINTS 16	/*!< Kantenlaenge einer Section in Punkten ==> eine Section braucht MAP_SECTION_POINTS*MAP_SECTION_POINTS Bytes  */
     #endif

    +/*!  Suchkreis (Botdurchmesser) in Mapfelder je nach Aufloesung umgerechnet*/
    +#define MAP_RADIUS_FIELDS_GODEST	  (BOT_DIAMETER * MAP_RESOLUTION / 100)	/*!< Umkreisfelder fuer Pfadsuche */
    +/*! hier gleich Defines definieren, um kostspielige Berechnungen zu sparen */
    +#if MAP_RADIUS_FIELDS_GODEST / 2 < 1
    +  #define MAP_RADIUS_FIELDS_GODEST_HALF 1  // sicherstellen dass nicht 0 auftritt
    +#else
    +  #define MAP_RADIUS_FIELDS_GODEST_HALF (MAP_RADIUS_FIELDS_GODEST / 2)
    +#endif
    +#define MAP_RADIUS_FIELDS_GODEST_HALF_QUAD (MAP_RADIUS_FIELDS_GODEST_HALF * MAP_RADIUS_FIELDS_GODEST_HALF) // halber Quadratradius
    +
    +#define MAPFIELD_IGNORE          20  /*!< negativer Schwellwert, bei dem Laut Map Hindernis gemeldet wird */
    +#define HAZPOT                   5   /*! hohe Hinderniswahrscheinlichkeit und trotzdem weiter beruecksichtigt */
    +#define MAP_ART_HAZARD           30  /*!< kuenstliches Hindernis in MAP fuer Pathplaning in lokalem Minimum */
    +
    +
     // Die folgenden Variablen/konstanten NICHT direkt benutzen, sondern die zugehoerigen Makros: get_map_min_x() und Co!
     // Denn sonst erhaelt man Karten und nicht Weltkoordinaten!
     #ifdef SHRINK_MAP_ONLINE
    @@ -123,8 +138,18 @@
      */
     float map_to_world(uint16 map_koord);

    +
     /*!
      * PrÃ¼ft ob eine direkte Passage frei von Hindernissen ist
    + * @param  from_x Startort x Kartenkoordinaten
    + * @param  from_y Startort y Kartenkoordinaten
    + * @param  to_x Zielort x Kartenkoordinaten
    + * @param  to_y Zielort y Kartenkoordinaten
    + * @return 1 wenn alles frei ist
    + */
    +uint8 map_way_free_fields(uint16 from_x, uint16 from_y, uint16 to_x, uint16 to_y);
    +/*!
    + * PrÃ¼ft ob eine direkte Passage frei von Hindernissen ist
      * @param  from_x Startort x Weltkoordinaten
      * @param  from_y Startort y Weltkoordinaten
      * @param  to_x Zielort x Weltkoordinaten
    @@ -134,6 +159,107 @@
     int8 map_way_free(float from_x, float from_y, float to_x, float to_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);
    +
    +/*!
    + * 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);
    +
    +
    +/*! 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);
    +
    +	/*! 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
    +	 */
    +	uint8 map_get_value_field_circle(uint16 x, uint16 y, uint8 radius, int8 value);
    +
    +/*!
    + * ermittelt ob der Wert val innerhalb des Umkreises mit Radius r von xy liegt; bei geringer MCU-Aufloesung direkter
    + * Vergleich mit xy
    + * @param xy  Map-Koordinaten
    + * @param radius Vergleichsradius (halber Suchkreis)
    + * @param val Vergleichswert
    + * @return True wenn Wert val gefunden sonst False
    + */
    +uint8 value_in_circle (uint16 x, uint16 y, uint8 radius, int8 val);
    +
    +/*! Routine ermittelt ab dem vom Mittelpunkt links/ rechts versetzten Punkt xp yp, in Blickrichtung
    + *  dist mm von den Abgrundsensoren voraus, die X-Mapkoordinate
    + *  verwendet zur Map-Lochmarkierung; 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 dist Abstand voraus in mm
    + * @return berechnete X-Mapkoordinate
    + */
    +uint16 get_mapposx_dist(float xp, float yp, float h, uint16 dist);
    +
    +/*! Routine ermittelt ab dem vom Mittelpunkt links/ rechts versetzten Punkt xp yp, in Blickrichtung
    + *  dist mm von den Abgrundsensoren voraus, die Y-Mapkoordinate
    + *  verwendet zur Map-Lochmarkierung; 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 dist Abstand voraus in mm
    + * @return berechnete X-Mapkoordinate
    + */
    +uint16 get_mapposy_dist(float xp, float yp, float h, uint16 dist) ;
    +
    +/*!
    + * liefert den Durschnittswert um einen Punkt der Karte herum
    + * @param x x-Ordinate der Karte
    + * @param y y-Ordinate der Karte
    + * @return Wert des Durchschnitts um das Feld (>0 heisst frei, <0 heisst belegt
    + */
    +int8 map_get_average_fields (uint16 x, uint16 y, uint16 radius);
    +
    +/*!
    + * Check, ob die MAP-Koordinate xy mit destxy identisch ist (bei kleinen Aufloesungen) oder sich
    + * innerhalb des halben Radius-Umkreises befindet (hoehere Aufloesungen); z.B. 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
    + */
    +uint8 map_in_dest (uint16 x, uint16 y, uint16 destx, uint16 desty);
    +
    +/*! Loescht die Mapfelder, die in einem bestimtmen Wertebereich min_val max_val liegen, d.h. diese
    + *  werden auf 0 gesetzt
    + *  @param min_val minimaler Wert
    + *  @param max_val maximaler Wert
    + */
    +void clear_map(int8 min_val, int8 max_val);
    +
    +/*!
      * Zeigt die Karte an
      */
     void print_map(void);
    Index: C:/botneu/ct-Bot/map.c
    ===================================================================
    --- C:/botneu/ct-Bot/map.c	(revision 1150)
    +++ C:/botneu/ct-Bot/map.c	(working copy)
    @@ -31,6 +31,7 @@
     #include "mini-fat.h"
     #include "mmc-vm.h"

    +
     #ifdef MAP_AVAILABLE
     #include <math.h>
     #include <stdlib.h>
    @@ -66,12 +67,14 @@
     #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-MAP_STEP_FREE)   /*!< Frei Aktualisierung nur bis zu diesem Wert wegen Pfadplanung */
    +
     #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 */
    @@ -366,7 +369,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) < FREE_BOUNDERY)  // nicht groesser als Grenzwert
    +    map_update_field(x,y,MAP_STEP_FREE);
     }

     /*!
    @@ -446,14 +450,10 @@
     		d=dist;

    -
    -	// Hinderniss, dass der Sensor sieht in Weltkoordinaten
    -	float PH_x = x + (DISTSENSOR_POS_FW + d) * cos(h);
    -	float PH_y = y + (DISTSENSOR_POS_FW + d) * sin(h);
    -	// Hinderniss, dass der linke Sensor sieht in Kartenkoordinaten
    -	uint16 PH_X = world_to_map(PH_x);
    -	uint16 PH_Y = world_to_map(PH_y);
    -
    +	// liefert die Mapkoordinaten im Abstand dist vom Punkt xy
    +	uint16 PH_X=get_mapposx_dist(x,y,h,dist);
    +	uint16 PH_Y=get_mapposy_dist(x,y,h,dist);
    +
     	if ((dist > 80 ) && (dist <SENS_IR_INFINITE))
     			map_update_occupied(PH_X,PH_Y);

    @@ -569,8 +569,8 @@
     	  for (i=0; i<dX; ++i) {
     		 for (w=-width; w<= width; w++) // wir mÃ¼ssen die ganze Breite des absuchen
     //			 map_set_field(lX+i*sX, lY+w,-126);
    -		     if (map_get_field(lX+i*sX, lY+w) <0) // ein hinderniss reicht fÃ¼r den Abbruch
    -		       return 0;
    +		     if (map_get_field(lX+i*sX, lY+w) < -MAPFIELD_IGNORE) // ein hinderniss reicht fÃ¼r den Abbruch
    +		       return False;

     	    lh += dY;
     	    if (lh >= dX) {
    @@ -583,8 +583,8 @@
     	  for (i=0; i<dY; ++i) {
     		 for (w=-width; w<= width; w++) // wir mÃ¼ssen die ganze Breite des absuchen
     //			 map_set_field(lX+w, lY+i*sY,126);
    -			 if (map_get_field (lX+w, lY+i*sY) <0 ) //ein hinderniss reicht fÃ¼r den Abbruch
    -		       return 0;
    +			 if (map_get_field (lX+w, lY+i*sY) <-MAPFIELD_IGNORE ) //ein hinderniss reicht fÃ¼r den Abbruch
    +		       return False;
     	    lh += dX;
     	    if (lh >= dY) {
     	      lh -= dY;
    @@ -593,7 +593,7 @@
     	  }
     	}

    -	return 1;
    +	return True;
     }

    @@ -603,12 +603,211 @@
      * @param  from_y Startort y Weltkoordinaten
      * @param  to_x Zielort x Weltkoordinaten
      * @param  to_y Zielort y Weltkoordinaten
    - * @return 1 wenn alles frei ist
    + * @return True wenn alles frei ist sonst False
      */
     int8 map_way_free(float from_x, float from_y, float to_x, float to_y){
     	return map_way_free_fields(world_to_map(from_x),world_to_map(from_y),world_to_map(to_x),world_to_map(to_y));
     }

    +	/*! 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
    +	 */
    +	uint8 map_get_value_field_circle(uint16 x, uint16 y, uint8 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
    +	}
    +
    +/*!
    + * ermittelt ob der Wert val innerhalb des Umkreises mit Radius r von xy liegt; bei geringer MCU-Aufloesung direkter
    + * Vergleich mit xy
    + * @param xy  Map-Koordinaten
    + * @param radius Vergleichsradius (halber Suchkreis)
    + * @param val Vergleichswert
    + * @return True wenn Wert val gefunden
    + */
    +uint8 value_in_circle (uint16 x, uint16 y, uint8 radius, int8 val) {
    +
    +  #if MAP_RADIUS_FIELDS_GODEST > 0
    +	  uint8 r;
    +      for (r=1; r<=radius; 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
    +}
    +
    +/*!
    + * Check, ob die MAP-Koordinate xy mit destxy identisch ist (bei kleinen Aufloesungen) oder sich
    + * innerhalb des halben Radius-Umkreises befindet (hoehere Aufloesungen); z.B. 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
    + */
    +uint8 map_in_dest (uint16 x, uint16 y, uint16 destx, uint16 desty) {
    +  // MAP_RADIUS_FIELDS_GODEST sind die Mapfelder im Botdurchmesser fuer Suchkreis
    +  #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
    +}
    +
    +#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
    +	      }
    +		}
    +	}
    +
    +#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           ;   // 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);
    +}
    +
    +
    +/*! Loescht die Mapfelder, die in einem bestimtmen Wertebereich min_val max_val liegen, d.h. diese
    + *  werden auf 0 gesetzt
    + *  @param min_val minimaler Wert
    + *  @param max_val maximaler Wert
    + */
    +void clear_map(int8 min_val, int8 max_val){
    +
    +		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);
    +
    +				if (tmp>=min_val && tmp<= max_val) // alles zwischen Intervall auf 0 setzen
    +				    map_set_field(x,y,0);
    +			}
    +		}
    +	}
    +
    +
    +/*! Routine ermittelt ab dem vom Mittelpunkt links/ rechts versetzten Punkt xp yp, in Blickrichtung
    + *  dist mm von den Abgrundsensoren voraus, die Mapkoordinaten XY
    + *  verwendet zur Map-Lochmarkierung; 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
    + */
    +uint16 get_mapposx_dist(float xp, float yp, float h, uint16 dist) {
    +
    +	// Hinderniss auf X-Koord, dass der Sensor sieht in Weltkoordinaten
    +	float PH_x = xp + (DISTSENSOR_POS_FW + dist) * cos(h);
    +
    +	// Hinderniss, welches der Sensor sieht in, umgerechnet in Karten-Map-Koordinaten
    +	return world_to_map(PH_x);
    +}
    +
    +uint16 get_mapposy_dist(float xp, float yp, float h, uint16 dist) {
    +
    +	// Hinderniss auf Y-Koord, dass der Sensor sieht in Weltkoordinaten
    +	float PH_y = yp + (DISTSENSOR_POS_FW + dist) * sin(h);
    +
    +	// Hinderniss, welches der Sensor sieht in, umgerechnet in Karten-Map-Koordinaten
    +	return  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 Abgrundsensoren
    +	PH_X=get_mapposx_dist(x,y,h,0);
    +	PH_Y=get_mapposy_dist(x,y,h,0);
    +
    +	// nur wenn noch nicht als Loch gekennzeichnet auf Umgebungskreis Loch vermerken
    +	if (map_get_field(PH_X,PH_Y) > -128)
    +			map_set_value_occupied(PH_X,PH_Y,-128);
    +
    +
    +}
    +
     #ifdef PC
     	/*!
     	 * Schreibt einbe Karte in eine PGM-Datei
    Index: C:/botneu/ct-Bot/pc/tcp.c
    ===================================================================
    --- C:/botneu/ct-Bot/pc/tcp.c	(revision 1150)
    +++ C:/botneu/ct-Bot/pc/tcp.c	(working copy)
    @@ -188,7 +188,7 @@
     	uint8 * ptr = data;

     	if ((sendBufferPtr + length) > sizeof(sendBuffer)){
    -		printf("%s() %s:%u: sendBuffer filled with %u/%lu Bytes, another %u bytes pending. Full! Aborting copy!\n",__FUNCTION__,__FILE__, __LINE__,sendBufferPtr,sizeof(sendBuffer),length);
    +		//printf("%s() %s:%u: sendBuffer filled with %u/%lu Bytes, another %u bytes pending. Full! Aborting copy!\n",__FUNCTION__,__FILE__, __LINE__,sendBufferPtr,sizeof(sendBuffer),length);

     		printf("  ==> Trying to recover by calling flushSendBuffer()\n");
     		flushSendBuffer();
    Index: C:/botneu/ct-Bot/Changelog.txt
    ===================================================================
    --- C:/botneu/ct-Bot/Changelog.txt	(revision 1150)
    +++ C:/botneu/ct-Bot/Changelog.txt	(working copy)
    @@ -1,5 +1,7 @@
     CHANGELOG fuer c't-Bot
     ======================
    +2007-05-19 Frank Menzel  [Menzelfr@xxxxxxx]: Pfadplanungsverhalten map_go_destination und informieren Ã¼ber Abgrund via Registrierung
    +
     2007-05-13 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Bugfix in motor-low.h, vorlaeufiger Workaround fuer solve_maze() und Kommentar in mmc-low.S korrigiert

     2007-05-09 Benjamin Benz [bbe@xxxxxxxx]: Bugfix in map.c
    Index: C:/botneu/ct-Bot/.cdtbuild
    ===================================================================
    --- C:/botneu/ct-Bot/.cdtbuild	(revision 1150)
    +++ C:/botneu/ct-Bot/.cdtbuild	(working copy)
    @@ -10,8 +10,8 @@
     <option id="gnu.c.compiler.option.include.paths.387475860" superClass="gnu.c.compiler.option.include.paths" valueType="includePath">
     <listOptionValue builtIn="false" value="&quot;${ProjDirPath}&quot;"/>
     <listOptionValue builtIn="false" value="&quot;${ProjDirPath}/include&quot;"/>
    -<listOptionValue builtIn="false" value="&quot;C:\Programme\MinGW\include&quot;"/>
    -<listOptionValue builtIn="false" value="&quot;C:\Programme\pthreads\pthreads.2&quot;"/>
    +<listOptionValue builtIn="false" value="&quot;C:\MinGW\include&quot;"/>
    +<listOptionValue builtIn="false" value="&quot;C:\pthreads\pthreads.2&quot;"/>
     </option>
     <option id="gnu.c.compiler.option.preprocessor.def.symbols.1823712582" superClass="gnu.c.compiler.option.preprocessor.def.symbols" valueType="definedSymbols">
     <listOptionValue builtIn="false" value="PC"/>
    @@ -27,8 +27,8 @@
     <listOptionValue builtIn="false" value="pthreadGC2"/>
     </option>
     <option id="gnu.c.link.option.paths.142594843" superClass="gnu.c.link.option.paths" valueType="stringList">
    -<listOptionValue builtIn="false" value="&quot;C:\Programme\pthreads\Pre-built.2\lib&quot;"/>
    -<listOptionValue builtIn="false" value="&quot;C:\Programme\MinGW\lib&quot;"/>
    +<listOptionValue builtIn="false" value="&quot;C:\pthreads\Pre-built.2\lib&quot;"/>
    +<listOptionValue builtIn="false" value="&quot;C:\MinGW\lib&quot;"/>
     </option>
     <option id="gnu.c.link.option.noshared.1812776571" superClass="gnu.c.link.option.noshared" value="true" valueType="boolean"/>
     </tool>
    Index: C:/botneu/ct-Bot/ui/gui.c
    ===================================================================
    --- C:/botneu/ct-Bot/ui/gui.c	(revision 1150)
    +++ C:/botneu/ct-Bot/ui/gui.c	(working copy)
    @@ -121,6 +121,12 @@
     	#ifdef RAM_DISPLAY_AVAILABLE
     		register_screen(&ram_display);
     	#endif
    +	// auch nur wenn Verhalten aktiv ist
    +	#ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
    +	 #ifdef DISPLAY_MAP_GO_DESTINATION
    +		register_screen(&mapgo_display);
    +	 #endif
    +	#endif
     }

     #endif	// DISPLAY_AVAILABLE
    Index: C:/botneu/ct-Bot/ui/available_screens.h
    ===================================================================
    --- C:/botneu/ct-Bot/ui/available_screens.h	(revision 1150)
    +++ C:/botneu/ct-Bot/ui/available_screens.h	(working copy)
    @@ -35,13 +35,15 @@

     #define SENSOR_DISPLAY_AVAILABLE		/*!< zeigt die Sensordaten an */
     //#define DISPLAY_REGELUNG_AVAILABLE		/*!< Gibt Debug-Infos der Motorregelung aus */
    -//#define DISPLAY_BEHAVIOUR_AVAILABLE		/*!< zeigt Verhalten an */
    +#define DISPLAY_BEHAVIOUR_AVAILABLE		/*!< zeigt Verhalten an */
     //#define MISC_DISPLAY_AVAILABLE			/*!< aehm ja, der Rest irgendwie... */
     #define DISPLAY_ODOMETRIC_INFO			/*!< zeigt Positions- und Geschwindigkeitsdaten an */
     #define DISPLAY_MMC_INFO				/*!< Zeigt die Daten der MMC-Karte an */
     //#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: C:/botneu/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs
    ===================================================================
    --- C:/botneu/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs	(revision 1150)
    +++ C:/botneu/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs	(working copy)
    @@ -1,13 +1,15 @@
    -#Thu Mar 29 15:23:55 CEST 2007
    -cdt.managedbuild.config.gnu.exe.debug.1197043799/internalBuilder/enabled=false
    -cdt.managedbuild.config.gnu.exe.debug.1197043799/internalBuilder/ignoreErr=true
    -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.1197043799=<?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.1197043799.2016949464=<?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/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?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.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.1197043799.2016949464=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n
    -environment/project=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment/>\n
    -environment/project/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment/>\n
    -environment/project/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable delimiter\="\:" name\="PATH" operation\="append" value\="/usr/local/avr/bin"/>\n</environment>\n
    +#Thu May 17 21:04:18 CEST 2007
    +cdt.managedbuild.config.gnu.exe.debug.1077176217/internalBuilder/enabled=false
    +cdt.managedbuild.config.gnu.exe.debug.1077176217/internalBuilder/ignoreErr=true
    +cdt.managedbuild.config.gnu.exe.debug.1197043799/internalBuilder/enabled=false
    +cdt.managedbuild.config.gnu.exe.debug.1197043799/internalBuilder/ignoreErr=true
    +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.1197043799=<?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.1197043799.2016949464=<?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/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?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.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.1197043799.2016949464=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n
    +environment/project=<?xml version\="1.0" encoding\="UTF-8"?>\r\n<environment/>\r\n
    +environment/project/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\r\n<environment/>\r\n
    +environment/project/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable delimiter\="\:" name\="PATH" operation\="append" value\="/usr/local/avr/bin"/>\n</environment>\n
    Index: C:/botneu/ct-Bot/ct-Bot.h
    ===================================================================
    --- C:/botneu/ct-Bot/ct-Bot.h	(revision 1150)
    +++ C:/botneu/ct-Bot/ct-Bot.h	(working copy)
    @@ -31,7 +31,7 @@
     /************************************************************
     * Module switches, to make code smaller if features are not needed
     ************************************************************/
    -//#define LOG_CTSIM_AVAILABLE		/*!< Logging zum ct-Sim (PC und MCU) */
    +#define LOG_CTSIM_AVAILABLE		/*!< Logging zum ct-Sim (PC und MCU) */
     //#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) */
    @@ -69,7 +69,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 ADJUST_PID_PARAMS		/*!< macht PID-Paramter zur Laufzeit per FB einstellbar */

- Antworten:
  - Re: [ct-bot] neues Pfadplanungs-Verhalten nach der Map -> ueberarbeitet
    - From: Timo Sandmann
  - Re: [ct-bot] neues Pfadplanungs-Verhalten nach der Map -> ueberarbeitet
    - From: Benjamin Benz

- Referenzen:
  - Re: AW: [ct-bot] neues Pfadplanungs-Verhalten nach der Map
    - From: Benjamin Benz

- Voriger (Datum): Re: [ct-bot] Notfallverhalten Idee..., Torsten Evers
- Nächster (Datum): Re: [ct-bot] neues Pfadplanungs-Verhalten nach der Map -> ueberarbeitet, Timo Sandmann
- Voriger (Thread): Re: [ct-bot] Notfallverhalten Idee..., Torsten Evers
- Nächster (Thread): Re: [ct-bot] neues Pfadplanungs-Verhalten nach der Map -> ueberarbeitet, Timo Sandmann
- Indizes:
  - Datum
  - Thread