Patches: pathplaning.patch

C:/botneu/ct-Bot/bot-logic/behaviour_avoid_border.c

@@ -37 +37 @@
         
         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

C:/botneu/ct-Bot/bot-logic/behaviour_map_go_destination.c

@@ +1 @@
+/*
+ * 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@gmx.net)
+ *  @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

C:/botneu/ct-Bot/bot-logic/bot-logik.c

@@ -71 +71 @@
 #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 +145 @@
 
         #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 +175 @@
         #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 +267 @@
 }
 
 /*!
+ * 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

C:/botneu/ct-Bot/bot-logic/behaviour_scan.c

@@ -40 +40 @@
 
         
         #ifdef MAP_AVAILABLE
+        
+
                 static float last_x, last_y, last_head;
                 
                 float diff_x = fabs(x_pos-last_x);
@@ -69 +71 @@
                                 print_map();
                         }
                 */      
-        #endif
+        #endif  //MAP_AVAILABLE
 }
 
 
@@ -141 +143 @@
 //      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

C:/botneu/ct-Bot/bot-logic/behaviour_gotoxy.c

@@ -25 +25 @@
 */
 
 #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 +51 @@
         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

C:/botneu/ct-Bot/include/bot-logic/behaviour_gotoxy.h

@@ -29 +29 @@
 #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 +51 @@
  * @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_*/

C:/botneu/ct-Bot/include/bot-logic/behaviour_map_go_destination.h

@@ +1 @@
+/*
+ * 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@gmx.net)
+ * @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 */

C:/botneu/ct-Bot/include/bot-logic/available_behaviours.h

@@ -9 +9 @@
 #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 +19 @@
 #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 +38 @@
  */
 #ifndef MAP_AVAILABLE
         #undef BEHAVIOUR_SCAN_AVAILABLE
+        #undef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE
 #endif
 
 #ifdef BEHAVIOUR_GOTOXY_AVAILABLE
@@ -100 +102 @@
 
 #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"

C:/botneu/ct-Bot/include/bot-logic/bot-logik.h

@@ -116 +116 @@
 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 +165 @@
  */
 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

C:/botneu/ct-Bot/include/bot-logic/behaviour_scan.h

@@ -42 +42 @@
  */
 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

C:/botneu/ct-Bot/include/bot-local.h

@@ -37 +37 @@
 
 #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 +106 @@
 
 
 /*! 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 */
 
 
 

C:/botneu/ct-Bot/include/map.h

@@ -51 +51 @@
         #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 +138 @@
  */
 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 +159 @@
 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);

C:/botneu/ct-Bot/map.c

@@ -31 +31 @@
 #include "mini-fat.h"
 #include "mmc-vm.h"
 
+
 #ifdef MAP_AVAILABLE
 #include <math.h>
 #include <stdlib.h>
@@ -66 +67 @@
 #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 +369 @@
  * @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 +450 @@
                 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 +569 @@
           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 +583 @@
           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 +593 @@
           }
         }
         
-        return 1;
+        return True;
 }
 
 
@@ -603 +603 @@
  * @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

C:/botneu/ct-Bot/pc/tcp.c

@@ -188 +188 @@
         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(); 

C:/botneu/ct-Bot/Changelog.txt

@@ -1 +1 @@
 CHANGELOG fuer c't-Bot
 ======================
+2007-05-19 Frank Menzel  [Menzelfr@gmx.net]: Pfadplanungsverhalten map_go_destination und informieren ÃŒber Abgrund via Registrierung
+
 2007-05-13 Timo Sandmann [mail@timosandmann.de]: Bugfix in motor-low.h, vorlaeufiger Workaround fuer solve_maze() und Kommentar in mmc-low.S korrigiert
 
 2007-05-09 Benjamin Benz [bbe@heise.de]: Bugfix in map.c

C:/botneu/ct-Bot/.cdtbuild

@@ -10 +10 @@
 <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 +27 @@
 <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>

C:/botneu/ct-Bot/ui/gui.c

@@ -121 +121 @@
         #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

C:/botneu/ct-Bot/ui/available_screens.h

@@ -35 +35 @@
 
 #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

C:/botneu/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs

@@ -1 @@
-#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

C:/botneu/ct-Bot/ct-Bot.h

@@ -31 +31 @@
 /************************************************************
 * 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 +69 @@
 
 #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 */