c't

c't-Projekte - Mailinglisten


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

[ct-bot] Pfadplanung in area

Absender: Frank Menzel
Datum: Mo, 27.10.2008 16:42:28


Hallo,
habe jetzt die Pfadplanung in das area-Verhalten eingebaut. Das
Area-Verhalten verwendet jetzt ein eigenes Array zur Speicherung der
Wegalternativen, siehe Mailingliste.
 
Gruß, Frank
 
Index: C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c
===================================================================
--- C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c	(revision 1498)
+++ C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c	(working copy)
@@ -42,12 +42,27 @@
 #include "map.h"
 #include "timer.h"
 #include "math_utils.h"
-#include "pos_stack.h"
 #include "log.h"
 
 
-//#define DEBUG_DRIVE_AREA	// Schalter fuer Debug-Code
+//#define DEBUG_MEASURE_TIME	    // Schalter fuer Debug-Code Zeitmessung Map-Zugriffszeit
 
+//#define DEBUG_BEHAVIOUR_AREA	// Schalter fuer Debugausgaben
+
+#ifndef LOG_AVAILABLE
+  #undef DEBUG_BEHAVIOUR_AREA
+#endif
+#ifndef DEBUG_BEHAVIOUR_AREA
+  #undef LOG_DEBUG
+  #define LOG_DEBUG(a, ...) {}
+#endif
+
+
+#ifdef BEHAVIOUR_PATHPLANING_AVAILABLE
+  #define GO_WITH_PATHPLANING  // auskommentieren falls ohne Pfadplanung
+#endif
+
+
 /*! nur alle X-Millisekunden Mapzugriff der Observer-Verhalten */
 #define	CORRECTION_DELAY	700
 
@@ -93,10 +108,8 @@
 
 /*! Strukturtyp fuer die beobachteten Nachbar-Bahnen; fuer Links und Rechts bestehend aus 2 Punkten */
 typedef struct {
-	int16_t point1x; /*!< Einzelne Punkte */
-	int16_t point1y;
-	int16_t point2x;
-	int16_t point2y;
+	position_t point1;
+	position_t point2;
 } trackpoint_t;
 
 /*! Nachbarbahnen links und rechts */
@@ -109,7 +122,127 @@
 /*! gemerkte letzte Position des Bots, um nur aller xx gefahrender mm auf Map zuzugreifen */
 static trackpoint_t observe_lastpos;
 
+
 /* ==================================================================================
+ * ===== Definition eines eigenen Linienspeichers zum Merken der Alternativstrecken =
+ * ==================================================================================*/
+
+/*! Implementierung eines eigenen Linienspeichers; Stack nicht verwendbar wegen dem sonst identischen Speicherbereich fuer Pfadplanung und drive_Stack-Verhalten  */
+
+/*!
+ * Linie wird definiert als Anfangs- und Endpunkt
+ */
+typedef struct {
+	position_t p1; /*!< Anfangspunkt der Linie */
+	position_t p2; /*!< Endpunkt der Linie */
+} line_t;
+
+#define LINE_STORE_SIZE	32                  /*!< Speichergroesse, muss durch 2 teilbar sein */
+static line_t line_stack[LINE_STORE_SIZE];	/*!< Datenspeicher */
+static uint8_t linecounter = 0;				/*!< Anzahl der gespeicherten Linien, bestehend jeweils aus Start- und Endpunkt */
+//uint8_t line_store_sp = 0;				    /*!< Zeiger auf Arrayindex */
+
+
+/*!
+ * Leert den Linienspeicher
+ */
+void line_store_clear(void) {
+	linecounter = 0;
+}
+
+/*!
+ * Speicher voll?
+ * @return True falls Speicher voll, sonst False
+ */
+static uint8_t linestore_is_full(void) {
+	return linecounter == LINE_STORE_SIZE;
+}
+
+/*!
+ * Speicher leer?
+ * @return True falls Speicher leer sonst False
+ */
+static uint8_t linestore_is_empty(void) {
+	return linecounter == 0;
+}
+
+/*!
+ * Speichern einer Linienkoordinate auf den Stack
+ * @param pos	     X/Y-Koordinaten des zu sichernden Punktes
+ * @param firstpoint True falls der 1. der beiden Linienpunkte uebergeben wurde, False fuer den anderen Linienpunkt 
+ * @return		True wenn erfolgreich sonst False wenn Array voll ist
+ */
+uint8_t line_store_push_point(position_t pos, uint8_t firstpoint) {
+	if (linestore_is_full()) {
+		return False;
+	}
+	if (firstpoint)  // den richtigen Punkt in die entsprechende Koordinate speichern
+	  line_stack[linecounter].p1 = pos;
+	else
+	  line_stack[linecounter].p2 = pos;
+  
+	linecounter++;
+	return True;
+}
+
+/*!
+ * Speichern einer Linie auf den Stack durch Ubergabe des Start- und Endpunktes
+ * @param p1	zu sichernder Startpunktes
+ * @param p2	zu sichernder Endpunkt
+ * @return		True wenn erfolgreich sonst False wenn Linienstack voll ist
+ */
+uint8_t line_store_push(position_t p1, position_t p2) {
+	if (!line_store_push_point(p1,True))  // 1. Linienpunkt in Stack
+	  return False;
+    if (!line_store_push_point(p2,False)) // 2. Linienpunkt in Stack
+	  return False;
+	
+	LOG_DEBUG("Linie gepusht: P1: %1d %1d P2: %1d %1d, linecounter %1d",p1.x,p1.y,p2.x,p2.y,linecounter);
+	return True;
+	
+	
+
+}
+
+/*!
+ * Pop-Routine zur Rueckgabe des letzten auf dem Linienstack gepushten Punktes
+ * @param *pos	     Zeiger auf Rueckgabewert der Linienkoordinate
+ * @param firstpoint True fuer den 1. Linienpunkt, Flase fuer den 2. Linienpunkt
+ * @return		False falls Pop nicht erfolgreich, d.h. kein Punkt mehr auf dem Stack, sonst True nach erfolgreichem Pop
+ */
+uint8_t line_store_pop_point(position_t * pos, uint8_t firstpoint) {
+	if (linestore_is_empty()) {
+		return False;
+	}
+	linecounter--;
+
+	*pos=(firstpoint)?line_stack[linecounter].p1:line_stack[linecounter].p2;
+     
+	return True;
+}
+
+/*!
+ * Pop-Routine zur Rueckgabe der letzten auf dem Stack gespeicherten Linie
+ * @param *pos1	Zeiger auf Rueckgabe-Speicher der 1. Linienkoordinate
+ * @param *pos2	Zeiger auf Rueckgabe-Speicher der 2. Linienkoordinate
+ * @return		False falls Pop nicht erfolgreich, d.h. kein Punkt mehr auf dem Stack, sonst True nach erfolgreichem Pop
+ */
+uint8_t line_store_pop(position_t * pos1, position_t * pos2) {
+	if (!line_store_pop_point(pos2,False))
+	  return False;
+	if (!line_store_pop_point(pos1,True))
+	  return False;
+	
+	LOG_DEBUG("Popline P1: %1d %1d P2: %1d %1d",pos1->x,pos1->y,pos2->x,pos2->y);
+	return True;
+}
+
+/* ==================================================================================
+ * ===== Ende des Linienstacks ======================================================
+ * ==================================================================================*/
+
+
+/* ==================================================================================
  * ===== Start der allgemein fuer diese Verhalten notwendigen Routinen ==============
  * ==================================================================================*/
 
@@ -123,89 +256,58 @@
 
 /*!
  * liefert den Wert eines Feldes als Durchschnittwert laut Map in bestimmtem Umkreis
- * @param x	X-Weltkoordinate
- * @param y	Y-Weltkoordinate
+ * @param point	XY-Weltkoordinate
  * @return	Durchschnittswert des Feldes (>0 heisst frei, <0 heisst belegt)
  */
-static int8_t map_get_field(int16_t x, int16_t y) {
+static int8_t map_get_field(position_t point) {
 #ifdef DEBUG_DRIVE_AREA
 	uint8_t result;
-	TIMER_MEASURE_TIME(result = map_get_average(x, y, 50));
+	TIMER_MEASURE_TIME(result = map_get_average(point.x, point.y, 50));
 	return result;
 #else
-	return map_get_average(x, y, 50);  // Mapwert Durchschnitt
+	return map_get_average(point.x, point.y, 50);  // Mapwert Durchschnitt
 #endif
 }
 
 /*!
  * Speichert eine Line auf dem Stack, d.h. den Start- und Endpunkt der Linie
- * @param x1	X-Koordinate des ersten Punktes der Linie
- * @param y1	Y-Koordinate des ersten Punktes der Linie
- * @param x2	X-Koordinate des zweiten Punktes der Linie
- * @param y2	Y-Koordinate des zweiten Punktes der Linie
+ * @param point1	XY-Koordinate des ersten Punktes der Linie
+ * @param point2	XY-Koordinate des zweiten Punktes der Linie
  */
-static void push_stack_pos_line(int16_t x1, int16_t y1, int16_t x2, int16_t y2) {
+static void push_stack_pos_line(position_t point1, position_t point2) {
 
-	if ((x1 == 0 && y1 == 0) || (x2 == 0 && y2 == 0))
+	if ((point1.x == 0 && point1.y == 0) || (point2.x == 0 && point2.y == 0))
 		return; // ungueltige Werte werden nicht uebernommen
 
-	// Push der Einzelpunkte
-	pos_store_push(x1, y1);
-	pos_store_push(x2, y2);
+	// Push der Linienpunkte
+	line_store_push(point1, point2);	
 }
 
-/*!
- * Holt eine Line vom Stack, d.h. die beiden Punkte der Linie werden zurueckgegeben
- * @param *x1	X-Koordinate des ersten Punktes der Linie
- * @param *y1	Y-Koordinate des ersten Punktes der Linie
- * @param *x2	X-Koordinate des zweiten Punktes der Linie
- * @param *y2	Y-Koordinate des zweiten Punktes der Linie
- * @return True wenn erfolgreich, False falls Stack leer ist
- */
-static uint8_t pop_stack_pos_line(int16_t * x1, int16_t * y1, int16_t * x2,
-		int16_t * y2) {
-	int16_t x_i;
-	int16_t y_i;
-	if (!pos_store_pop(&x_i, &y_i))
-		return False;
-	*x1 = x_i;
-	*y1 = y_i;
 
-	if (!pos_store_pop(&x_i, &y_i))
-		return False;
-	*x2 = x_i;
-	*y2 = y_i;
-	return True;
-}
-
 /*!
  * Hilfsroutine, um 2 Punkte in die Merkvariable zu speichern
- * @param *lastpointx X-Merkvariable
- * @param *lastpointy Y-Merkvariable
+ * @param *lastpoint XY-Merkvariable
  * @param mapx zu merkender X-Wert
  * @param mapy zu merkender Y-Wert
  */
-static void set_point_to_lastpoint(int16_t * lastpointx, int16_t * lastpointy,
-		int16_t mapx, int16_t mapy) {
-	*lastpointx = mapx;
-	*lastpointy = mapy;
+static void set_point_to_lastpoint(position_t * lastpoint, int16_t mapx, int16_t mapy) {
+	lastpoint->x = mapx;
+	lastpoint->y = mapy;
 }
 
 /*!
  * Liefert True, wenn der Abstand zwischen den beiden Punkten gueltig ist, d.h. erreicht oder ueberschritten wurde
- * @param xs World-Koordinate des zu untersuchenden Punktes
- * @param ys World-Koordinate des zu untersuchenden Punktes
- * @param xd World-Koordinate des Zielpunktes
- * @param yd World-Koordinate des Zielpunktes
+ * @param point_s World-XY-Koordinate des zu untersuchenden Punktes
+ * @param point_d World-XY-Koordinate des Zielpunktes
  * @return True bei Erreichen des Abstandes zwischen den beiden Punkten sonst False
  */
-static uint8_t dist_valid(int16_t xs, int16_t ys, int16_t xd, int16_t yd) {
+static uint8_t dist_valid(position_t point_s, position_t point_d) {
 	// falls ein Punktepaar noch 0 hat, so ist dies ungueltig
-	if ((xs == 0 && ys == 0) || (xd == 0 && yd == 0))
+	if ((point_s.x == 0 && point_s.y == 0) || (point_d.x == 0 && point_d.y == 0))
 		return False;
 
 	/* Abstandsermittlung nach dem guten alten Pythagoras ohne Ziehen der Wurzel */
-	return (get_dist(xs, ys, xd, yd) > DIST_VALID_QUAD);
+	return (get_dist(point_s.x, point_s.y, point_d.x, point_d.y) > DIST_VALID_QUAD);
 }
 
 /*!
@@ -216,20 +318,19 @@
  * @param *point 	berechnete Map-Weltkoordinate
  * @return 			Mapwert an dem ermittelten Punkt in Blickrichtung zum Abstand dist
  */
-static int8_t getpoint_side_dist(uint16_t sidedist, int16_t dist, int8_t side,
-		position_t * point) {
+static int8_t getpoint_side_dist(uint16_t sidedist, int16_t dist, int8_t side, position_t * point) {
 	// Berechnung je nach zu blickender Seite des Bots
 	*point = (side == TRACKLEFT) ? calc_point_in_distance(heading, DISTSENSOR_POS_FW
 			+ dist, sidedist) : calc_point_in_distance(
 			heading, DISTSENSOR_POS_FW + dist, -sidedist);
 
 	// Rueckgabe des Mapwertes
-#ifdef DEBUG_DRIVE_AREA
+#ifdef DEBUG_MEASURE_TIME
 	uint8_t result;
-	TIMER_MEASURE_TIME(result = map_get_field(point->x, point->y));
+	TIMER_MEASURE_TIME(result = map_get_field(*point));
 	return result;
 #else
-	return map_get_field(point->x, point->y);
+	return map_get_field(*point);
 #endif
 }
 
@@ -241,19 +342,18 @@
 
 
 /*! Prueft ob der Bot schon eine bestimmte Strecke gefahren ist seit dem letzten Observerdurchgang
- * @param  *last_xpoint   letzte gemerkte X-Koordinate
- * @param  *last_ypoint   letzte gemerkte Y-Koordinate
+ * @param  *last  letzte gemerkte XY-Koordinate
  * @return True, wenn Bot schon gewisse Strecke gefahren ist und Map zu checken ist sonst False
  */
-static uint8_t check_map_after_distance(int16_t * last_xpoint, int16_t * last_ypoint) {
+static uint8_t check_map_after_distance(position_t * last) {
 
         // Abstand seit letztem Observerlauf ermitteln
-        uint16_t diff = get_dist(x_pos, y_pos, *last_xpoint, *last_ypoint);
+        uint16_t diff = get_dist(x_pos, y_pos, last->x, last->y);
 
 		//erst nach gewissem Abstand oder gleich bei noch initialem Wert Mappruefung
-		if ((diff >= MAPACCESS_AFTER_DISTANCE_QUAD) || (*last_xpoint==0 && *last_ypoint==0)) {
-			*last_xpoint = x_pos;
-            *last_ypoint = y_pos;
+		if ((diff >= MAPACCESS_AFTER_DISTANCE_QUAD) || (&last->x==0 && &last->y==0)) {
+			last->x = x_pos;
+            last->y = y_pos;
             return True;
 		}
 	return False;
@@ -281,8 +381,8 @@
 	}
 
     // nur wenn bot seit letztem Zugriff gewisse Strecke gefahren ist erfolgt Mapzugriff; Check erfolgt seitenabhaengig
-    uint8_t mapaccess = (checkside == TRACKLEFT) ? check_map_after_distance(&observe_lastpos.point1x, &observe_lastpos.point1y) :
-    	check_map_after_distance(&observe_lastpos.point2x,&observe_lastpos.point2y);
+    uint8_t mapaccess = (checkside == TRACKLEFT) ? check_map_after_distance(&observe_lastpos.point1) :
+    	check_map_after_distance(&observe_lastpos.point2);
 
     // Schluss bei Endeanforderung oder noch nicht weit genug gefahren
 	if (endrequest || !mapaccess) {
@@ -300,8 +400,8 @@
 
 	// hier ist also der Trackpunkt frei, dann merken und zur Endpunktsuche im Observer
 	// zuweisen der Initwerte in die richtigen Puntvars
-	set_point_to_lastpoint(&observe->point1x, &observe->point1y, map_pos.x, map_pos.y);
-	set_point_to_lastpoint(&observe->point2x, &observe->point2y, 0, 0);
+	set_point_to_lastpoint(&observe->point1, map_pos.x, map_pos.y);
+	set_point_to_lastpoint(&observe->point2, 0, 0);
 
     // Startpunkt gefunden
 	return True;
@@ -324,7 +424,7 @@
  * @return True falls Endpunkt auf der Seite ermittelt werden konnte sonst False
  */
 static uint8_t observe_get_endpoint(int8_t checkside, uint8_t * behavstate,
-		trackpoint_t * observer, int16_t * lastpointx, int16_t * lastpointy) {
+		trackpoint_t * observer, position_t * lastpoint) {
 	position_t map_pos;
 	int8_t mapval = 0;
 
@@ -333,8 +433,8 @@
 
 
 	// nur wenn bot seit letztem Zugriff gewisse Strecke gefahren ist erfolgt Mapzugriff
-    uint8_t mapaccess = (checkside == TRACKLEFT) ? check_map_after_distance(&observe_lastpos.point1x, &observe_lastpos.point1y) :
-    	check_map_after_distance(&observe_lastpos.point2x, &observe_lastpos.point2y);
+    uint8_t mapaccess = (checkside == TRACKLEFT) ? check_map_after_distance(&observe_lastpos.point1) :
+    	check_map_after_distance(&observe_lastpos.point2);
 
 	// weitere Checks nur wenn gewisse Strecke gefahren oder Endeanforderung besteht
     if (!endrequest && !mapaccess)
@@ -345,22 +445,22 @@
 			&map_pos);
 
 	//Endepunktsuche nicht weiter verfolgen falls noch kein Startpunkt vorliegt
-	if ((observer->point1x == 0 && observer->point1y == 0)) {
+	if ((observer->point1.x == 0 && observer->point1.y == 0)) {
 		*behavstate = (endrequest) ? TRACK_END : 0; // weiter mit Startpunktsuche oder Ende
 		return False; //Endepunktsuche ungueltig
 	}
 
     //Hindernis oder schon befahren erkannt aber noch kein gueltiger Zwischenendpunkt ermittelt; Ende falls
     //Endeanforderung kam sonst weiter mit Startpunktsuche
-    if ((mapval < 0 || mapval >= MAP_TRACKVAL) && *lastpointx == 0 && *lastpointy == 0) {
-			set_point_to_lastpoint(&observer->point1x, &observer->point1y, 0, 0);
+    if ((mapval < 0 || mapval >= MAP_TRACKVAL) && &lastpoint->x == 0 && &lastpoint->y == 0) {
+			set_point_to_lastpoint(&observer->point1, 0, 0);
             *behavstate = (endrequest) ? TRACK_END : 0;// weiter mit Startpunktsuche
-            set_point_to_lastpoint(lastpointx, lastpointy, 0, 0);
+            set_point_to_lastpoint(lastpoint, 0, 0);
 			return False; //Endepunktsuche ungueltig
 		}
 
     // Abstand zum Startpunkt muss gueltig sein sonst ist Schluss
-	if (!(dist_valid(observer->point1x, observer->point1y, map_pos.x, map_pos.y))) {
+	if (!(dist_valid(observer->point1, map_pos))) {
 			*behavstate = (endrequest) ? TRACK_END : 1;// bleibt in Endepunktsuche wenn nicht Endeanforderung kam
 			return False; //Endepunktsuche ungueltig
 		}
@@ -371,29 +471,29 @@
 
 		// bei Hindernis auf aktuellem Punkt wird der letzte gemerkte Nicht-Hindernispunkt auf den Stack gespeichert, der
 		// vorher auf Gueltigkeit gecheckt wird, d.h. muss bestimmten Mindestabstand zum Startpunkt haben und kein Hindernis sein
-		set_point_to_lastpoint(&observer->point2x, &observer->point2y, *lastpointx, *lastpointy);
+		set_point_to_lastpoint(&observer->point2, lastpoint->x, lastpoint->y);
 
 		//zuletzt gueltigen Endpunkt auf Stack sichern
-		push_stack_pos_line(observer->point1x, observer->point1y,*lastpointx, *lastpointy);
+		push_stack_pos_line(observer->point1, *lastpoint);
+		LOG_DEBUG("Push Endepunkt Hind or Track  Seite %1d P: %1d %1d",checkside,lastpoint->x, lastpoint->y);
 
 		//letzten gemerkten Punkt wieder als ungueltig kennzeichnen
-	    set_point_to_lastpoint(lastpointx, lastpointy, 0, 0);
+	    set_point_to_lastpoint(lastpoint, 0, 0);
 
 	} else {
 		//Punkt auf Nebenbahn hat hier keine Hinderniswahrscheinlichkeit und ist noch nicht befahren
 
 		// aktuellen gueltigen Zwischenpunkt der Nebenbahn merken; wird als Endepunkt genommen bei Hindernis auf Nebenbahn
-		set_point_to_lastpoint(lastpointx, lastpointy, map_pos.x, map_pos.y);
+		set_point_to_lastpoint(lastpoint, map_pos.x, map_pos.y);
 
 		// falls sich bei diesem gueltigen Endpunkt herausstellt, dass der vorhin ermittelte Startpunkt nun Hinderniswert besitzt, so wird der Endpunkt zum Startpunkt
 		// und weiter mit Endpunktsuche
-		if (map_get_field(observer->point1x, observer->point1y) < 0) {
-			set_point_to_lastpoint(&observer->point1x, &observer->point1y,
-					*lastpointx, *lastpointy); // Startpunkt ueberschreiben
+		if (map_get_field(observer->point1) < 0) {
+			set_point_to_lastpoint(&observer->point1, lastpoint->x, lastpoint->y); // Startpunkt ueberschreiben
 		    *behavstate = (endrequest) ? TRACK_END : 1;// bleibt in Endepunktsuche wenn nicht Endeanforderung kam
 
-			set_point_to_lastpoint(lastpointx, lastpointy, 0, 0);
-			set_point_to_lastpoint(&observer->point2x, &observer->point2y, 0, 0);
+			set_point_to_lastpoint(lastpoint, 0, 0);
+			set_point_to_lastpoint(&observer->point2, 0, 0);
 			return True; // gueltiger Endepunkt ermittelt
 		}
 
@@ -402,10 +502,12 @@
 			return False;
 
 		// gueltigen Endepunkt in Observervar vermerken
-		set_point_to_lastpoint(&observer->point2x, &observer->point2y, *lastpointx, *lastpointy);
+		set_point_to_lastpoint(&observer->point2, lastpoint->x, lastpoint->y);
 
 		//Merken der Strecke bei Endanforderung auf dem Stack
-		push_stack_pos_line(observer->point1x, observer->point1y, *lastpointx, *lastpointy);
+		push_stack_pos_line(observer->point1, *lastpoint);
+		
+		LOG_DEBUG("Push Endanf. Seite %1d P: %1d %1d",checkside,observer->point1.x,observer->point1.y);
 
 	}
 
@@ -413,15 +515,14 @@
 	*behavstate = (endrequest) ? TRACK_END : 0;
 
 	// solange die Startpunktsuche keinen neuen Startpunkt ermittelt hat, ist dieser initial
-	set_point_to_lastpoint(&observer->point1x, &observer->point1y, 0, 0);
+	set_point_to_lastpoint(&observer->point1, 0, 0);
 
 	return True; // Streckenpunkt ist hier gueltig
 
 }
 
 /*! Merkvariable des letzten gueltigen Endpunktes fuer Obserververhalten linke Spur */
-static int16_t lastpointleft_x = 0;
-static int16_t lastpointleft_y = 0;
+static position_t lastpointleft = { 0, 0 };
 
 /*!
  * Observer links; jeweils ein selbstaendiges Verhalten, welches die Nachbarbahn beobachtet und eine befahrbare Strecke bis zu einem Hindernis
@@ -446,15 +547,13 @@
 		// Anfangspunkt wurde bestimmt und ist hier auf der Suche nach dem Endpunkt; ein Endpunkt wird als Ende der Strecke
 		// vom bereits vorhandenen Startpunkt erkannt, wenn der Botbereich neben dem Bot nicht mehr befahren werden kann; es
 		// wird dann der zuletzt gemerkte befahrbare Punkt als Endpunkt genommen
-		if (!observe_get_endpoint(CHECK_SIDE, &observe1_state,
-				&observe_trackleft, &lastpointleft_x, &lastpointleft_y))
+		if (!observe_get_endpoint(CHECK_SIDE, &observe1_state, &observe_trackleft, &lastpointleft))
 			break;
 
 		break;
 	default:
-		if (observe_trackleft.point2x == 0 && observe_trackleft.point2y == 0)
-			set_point_to_lastpoint(&observe_trackleft.point1x,
-					&observe_trackleft.point1y, 0, 0);
+		if (observe_trackleft.point2.x == 0 && observe_trackleft.point2.y == 0)
+			set_point_to_lastpoint(&observe_trackleft.point1, 0, 0);
 
 		return_from_behaviour(data);
 		break;
@@ -470,19 +569,16 @@
 		switch_to_behaviour(caller, bot_observe_left_behaviour, OVERRIDE);
 		observe1_state = 0;
 		endrequest = False;
-		set_point_to_lastpoint(&observe_trackleft.point1x,
-				&observe_trackleft.point1y, 0, 0);
-		set_point_to_lastpoint(&observe_trackleft.point2x,
-				&observe_trackleft.point2y, 0, 0);
-		set_point_to_lastpoint(&lastpointleft_x, &lastpointleft_y, 0, 0);
-		set_point_to_lastpoint(&observe_lastpos.point1x, &observe_lastpos.point1y, 0, 0);
+		set_point_to_lastpoint(&observe_trackleft.point1, 0, 0);
+		set_point_to_lastpoint(&observe_trackleft.point2, 0, 0);
+		set_point_to_lastpoint(&lastpointleft, 0, 0);
+		set_point_to_lastpoint(&observe_lastpos.point1, 0, 0);
 
 	}
 }
 
 /*! Merkvariable des letzten gueltigen Endpunktes fuer Obserververhalten rechte Spur */
-static int16_t lastpointright_x = 0;
-static int16_t lastpointright_y = 0;
+static position_t lastpointright = { 0, 0 };
 
 /*!
  * Observer rechts; jeweils ein selbstaendiges Verhalten, welches die Nachbarbahn beobachtet und eine befahrbare Strecke bis zu einem Hindernis
@@ -508,15 +604,13 @@
 		// Anfangspunkt wurde bestimmt und ist hier auf der Suche nach dem Endpunkt; ein Endpunkt wird als Ende der Strecke
 		// vom bereits vorhandenen Startpunkt erkannt, wenn der Botbereich neben dem Bot nicht mehr befahren werden kann; es
 		// wird dann der zuletzt gemerkte befahrbare Punkt als Endpunkt genommen
-		if (!observe_get_endpoint(CHECK_SIDE, &observe2_state,
-				&observe_trackright, &lastpointright_x, &lastpointright_y))
+		if (!observe_get_endpoint(CHECK_SIDE, &observe2_state, &observe_trackright, &lastpointright))
 			break;
 
 		break;
 	default:
-		if (observe_trackright.point2x == 0 && observe_trackright.point2y == 0)
-			set_point_to_lastpoint(&observe_trackright.point1x,
-					&observe_trackright.point1y, 0, 0);
+		if (observe_trackright.point2.x == 0 && observe_trackright.point2.y == 0)
+			set_point_to_lastpoint(&observe_trackright.point1, 0, 0);
 
 		return_from_behaviour(data);
 		break;
@@ -531,12 +625,10 @@
 	if (!behaviour_is_activated(bot_observe_right_behaviour)) {
 		switch_to_behaviour(caller, bot_observe_right_behaviour, OVERRIDE);
 		observe2_state = 0;
-		set_point_to_lastpoint(&observe_trackright.point1x,
-				&observe_trackright.point1y, 0, 0);
-		set_point_to_lastpoint(&observe_trackright.point2x,
-				&observe_trackright.point2y, 0, 0);
-		set_point_to_lastpoint(&lastpointright_x, &lastpointright_y, 0, 0);
-		set_point_to_lastpoint(&observe_lastpos.point2x, &observe_lastpos.point2y, 0, 0);
+		set_point_to_lastpoint(&observe_trackright.point1, 0, 0);
+		set_point_to_lastpoint(&observe_trackright.point2, 0, 0);
+		set_point_to_lastpoint(&lastpointright, 0, 0);
+		set_point_to_lastpoint(&observe_lastpos.point2, 0, 0);
 
 	}
 
@@ -579,7 +671,7 @@
 /* Check-Routine zum Erkennen eines Hindernisses nach den Abgrund- als auch Abstandssensoren
  * @return True wenn Hindernis voraus sonst False
  */
-static uint8_t check_haz_in_map(void) {
+static uint8_t check_haz_sensDist(void) {
 	if (border_fired)
 		return True;
 
@@ -600,26 +692,25 @@
  * @param y2 Y-Koordinate des Punktes 2
  * @param change_points Punkte1 und 2 in nextline werden bei True vertauscht sonst belegt mit den Koordinaten
  */
-static void set_nextline(int16_t x1, int16_t y1, int16_t x2, int16_t y2,
-		uint8_t change_points) {
+static void set_nextline(position_t point1, position_t point2, uint8_t change_points) {
 	if (change_points) {
-		int16 xtemp = nextline.point1x;
-		int16 ytemp = nextline.point1y;
-		nextline.point1x = nextline.point2x;
-		nextline.point1y = nextline.point2y;
-		nextline.point2x = xtemp;
-		nextline.point2y = ytemp;
+		int16 xtemp = nextline.point1.x;
+		int16 ytemp = nextline.point1.y;
+		nextline.point1.x = nextline.point2.x;
+		nextline.point1.y = nextline.point2.y;
+		nextline.point2.x = xtemp;
+		nextline.point2.y = ytemp;
 	} else {
 		// der Punkt in der Naehe wird richtig nach nextline uebernommen; der naheste nach Punkt1
-		if (get_dist(x1, y1, x_pos, y_pos) < get_dist(x2, y2, x_pos, y_pos)) {
+		if (get_dist(point1.x, point1.y, x_pos, y_pos) < get_dist(point2.x, point2.y, x_pos, y_pos)) {
 
-			set_point_to_lastpoint(&nextline.point1x, &nextline.point1y, x1, y1);
-			set_point_to_lastpoint(&nextline.point2x, &nextline.point2y, x2, y2);
+			set_point_to_lastpoint(&nextline.point1, point1.x, point1.y);
+			set_point_to_lastpoint(&nextline.point2, point2.x, point2.y);
 
 		} else {
 			// der Punkt in der Naehe wird richtig nach nextline uebernommen; der naheste nach Punkt1
-			set_point_to_lastpoint(&nextline.point1x, &nextline.point1y, x2, y2);
-			set_point_to_lastpoint(&nextline.point2x, &nextline.point2y, x1, y1);
+			set_point_to_lastpoint(&nextline.point1, point2.x, point2.y);
+			set_point_to_lastpoint(&nextline.point2, point1.x, point1.y);
 		}
 	}
 }
@@ -655,8 +746,8 @@
 
 	position_t pos;
 
-	uint8_t free1;
-	uint8_t free2;
+	//uint8_t free1;
+	//uint8_t free2;
 
 	deactivateBehaviour(bot_cancel_behaviour_behaviour);	// Cancel-Verhalten abbrechen, damit es nicht ewig weiterlaeuft
 
@@ -666,22 +757,21 @@
 
         BLOCK_BEHAVIOUR(data, 500); // etwas warten
 
-		// anzufahrende naechste Strecke erst mal initialisieren
-		set_point_to_lastpoint(&nextline.point1x, &nextline.point1y, 0, 0);
-		set_point_to_lastpoint(&nextline.point2x, &nextline.point2y, 0, 0);
-
-
-		if (check_haz_in_map() || (sensDistL <= 200 || sensDistR <= 200)) { //gar nicht erst fahren bei <20cm Hindernis
+		 if (check_haz_sensDist() || (sensDistL <= 200 || sensDistR <= 200)) { //gar nicht erst fahren bei <20cm Hindernis
+			// TODO: Diese Strecke nicht gleich verwerfen sondern evtl untersuchen und Teilstrecken bilden; evtl. mit Pfadplanung andere Teilstrecke anfahren
+			// oder erst einmal mit anderem Weg tauschen, der gerade frei ist.
+			// da jetzt auch eigener Linienstack verwendet wird, koennte man einen Zaehler mitfuehren wie oft eine Strecke schon zurueckgestellt wurde und erst ab
+			// einem Schwellwert wirklich verwerfen 
+	
 			track_state = GET_LINE_FROM_STACK;
+			LOG_DEBUG("zu Stackholen wg. Abstand %1d %1d",sensDistL,sensDistR);
 			break;
-		}
+		 }
 
        //naechster Verhaltenszustand nach diesen Pruefungen; notwendig damit Observer bereits
        //laufen vor direktem Losfahren
         track_state = GO_FORWARD;
 
-		//Observer starten zum Beobachten der Nebenspuren
-		start_observe_left_right();
 
 		//wird jetzt zum Startzeitpunkt des Vorausfahrens bereits was gesehen, dann in Abhaengigkeit davon
 		//einen Punkt in der Bahn voraus berechnen um Wegfreiheit laut Map zu bestimmen
@@ -693,16 +783,29 @@
          else
           getpoint_side_dist(0, 250, TRACKLEFT, &pos); //nur etwas voraus Punkt berechnen wenn weiter gesehen
 
+         LOG_DEBUG("Abstaende voraus: L/R: %1d %1d",sensDistL,sensDistR);
+         
          // Wegfreiheit bis zum Punkt auf Strecke voraus bestimmen; manchmal kein Hind gesehen aber schon in Map
-         free1 = map_way_free(x_pos, y_pos, pos.x,pos.y);
 
-	     // ist der Weg voraus nicht frei laut Map, dann gar nicht erst fahren und verwerfen
-		 if (!free1) {
+	     // ist der Weg voraus nicht frei laut Map bis zum berechneten Punkt, dann gar nicht erst fahren 
+	     // Pfadplanung bringt hier gar nichts, da hier ja eine Bahn auf gerader Strecke, eben eine Nebenbahhn, befahren werden soll
+	     // Todo: Teilstrecken berechnen bis zum Hindernis selbst und ab Hindernis bis zum eigentlichen Endpunkt
+		 if (!map_way_free(x_pos, y_pos, pos.x,pos.y)) {  // rein optisch ist manchmal der Weg trotzdem frei-Mapverschiebung ?
+			// TODO: Diese Strecke nicht gleich verwerfen sondern evtl untersuchen und Teilstrecken bilden; evtl. mit Pfadplanung andere Teilstrecke anfahren
+			// oder erst einmal mit anderem Weg tauschen, der gerade frei ist.
+			// da jetzt auch eigener Linienstack verwendet wird, koennte man einen Zaehler mitfuehren wie oft eine Strecke schon zurueckgestellt wurde und erst ab
+			// einem Schwellwert wirklich verwerfen 
 			track_state = GET_LINE_FROM_STACK;
-			LOG_DEBUG("Weg versperrt");
+			LOG_DEBUG("Weg versperrt zu Zwischenpunkt %1d %1d, Endpunkt %1d %1d",pos.x,pos.y,nextline.point2.x,nextline.point2.y);			
 			break;
 		  }
+		// anzufahrende naechste Strecke erst mal initialisieren  
+        set_point_to_lastpoint(&nextline.point2, 0, 0);
+		set_point_to_lastpoint(&nextline.point1, 0, 0);
 
+		//Observer starten zum Beobachten der Nebenspuren
+		start_observe_left_right();
+
 		break;
 
     case GO_FORWARD:  //nach bestandenen Pruefungen fuer Bahn voraus nun vorwaertsfahren
@@ -716,7 +819,7 @@
 		    bot_goto_obstacle(data, DIST_AWARD_HAZ, 0); //mit hohem Abstand voraus via obstacle
 
         //damit auch Abgrund erkannt wird beim Vorfahren; obige Verhalten benutzen beide goto_pos
-		bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_haz_in_map);
+		bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_haz_sensDist);
         break;
 
 	case DELAY_AFTER_FORWARD:
@@ -764,9 +867,9 @@
 		backward=False; //init. vor dem naechsten Verhaltenszustand
 
 		//Drehen des Bots zum nahesten Punkt bei Gueltigkeit desselben
-		if (nextline.point1x != 0 || nextline.point1y != 0)
-			bot_turn(data, calc_angle_diff(nextline.point1x - x_pos,
-					nextline.point1y - y_pos));
+		if (nextline.point1.x != 0 || nextline.point1.y != 0)
+			bot_turn(data, calc_angle_diff(nextline.point1.x - x_pos,
+					nextline.point1.y - y_pos));
 
 		break;
 
@@ -779,18 +882,19 @@
         //Nach Drehung links und Abstand rechts ist zu klein (oder umgedreht) rueckwaerts fahren; der Punkt nebenan ist etwas vor seitlich und der bot dreht sich
         //somit zum Punkt aber etwas schraeg und der rechte Sensor ist etwas weiter weg als der linke (oder umgedreht), der bei zu nah unendlich liefert; also der von Wand weitere Sensor
         //wird zur Pruefung herangezogen, da bei zu nah der andere keinen gueltigen sinnvollen Wert liefert
-        if (!backward && (sensDistR <= DIST_AWARD_HAZ || sensDistL <= DIST_AWARD_HAZ)) {
+         if (!backward && (sensDistR <= DIST_AWARD_HAZ || sensDistL <= DIST_AWARD_HAZ)) {
          	//etwas zurueck um Map zu checken
-        		bot_drive_distance(data, 0, -BOT_SPEED_NORMAL, 5);  // rueckwaerts bei Abgrund
+         	LOG_DEBUG("zu Nah->Back L: %1d R: %1d",sensDistL,sensDistR);       	
+          		bot_drive_distance(data, 0, -BOT_SPEED_NORMAL, 5);  // rueckwaerts bei Abgrund
         		//direct_choice=0;//beim naechsten Mal immer noch zu nah Stackholen
         		backward=True;  // Kennung auf rueckwaerts gefahren setzen, da nur 1x erlaubt
         		break;// in aktuellem Eingang drin bleiben
-        }
+         }
 
         // Nach Ausrichtung zum Ziel darf kein Hind. in geringem Abstand voraus zu sehen sein
-        if (check_haz_in_map()) {
+        if (check_haz_sensDist()) {
         	track_state = GET_LINE_FROM_STACK;
-        	LOG_DEBUG("Hind voraus");
+        	LOG_DEBUG("Hind voraus L: %1d R: %1d",sensDistL,sensDistR);           	  	
          	break;
         }
 
@@ -798,7 +902,7 @@
 		track_state = GOTO_FIRST_DEST;
 
 		//Abstand des bots zum ersten Zielpunkt bestimmen
-		int16_t dist_to_point = sqrt(get_dist(nextline.point1x, nextline.point1y, x_pos, y_pos));
+		int16_t dist_to_point = sqrt(get_dist(nextline.point1.x, nextline.point1.y, x_pos, y_pos));
 
 		//Kennung fuer grossen Abstand zum Zielpunkt init.
         go_long_distance = (dist_to_point>150) ? True : False;
@@ -813,7 +917,7 @@
 			bot_goto_dist(data, dist_to_point - 400, 1); // bis 400mm vor Ziel
 
 			//damit bot nicht auf seinem weiten Weg zum Zielpunkt in den Abgrund faellt
-			bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_haz_in_map);
+			bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_haz_sensDist);
 		}
 
 		break;
@@ -824,7 +928,7 @@
         //zum Ziel ist; damit diese Strecke nicht verloren geht, Weg auf Stack nehmen
 
 		// Bot hat sein Ziel erreicht ohne wegen Hindernis voraus Fahrt abgebrochen zu haben
-		if (!check_haz_in_map()) {
+		if (!check_haz_sensDist()) {
 			track_state = GOTO_FIRST_DEST; //naechster Zustand ohne Abbruch
 			break;
 		}
@@ -835,12 +939,9 @@
 		static trackpoint_t p_temp; //zum Merken beim Vertauschen
 		//gibt es noch einen Stackeintrag, so wird die aktuell anzufahrende Strecke (nextline) auf den Stack gelegt und darueber
 		//als gueltige naechste Strecke wieder den letzten Stackeintrag
-		if (pop_stack_pos_line(&p_temp.point1x, &p_temp.point1y,
-				&p_temp.point2x, &p_temp.point2y)) {
-			push_stack_pos_line(nextline.point1x, nextline.point1y,
-					nextline.point2x, nextline.point2y);
-			push_stack_pos_line(p_temp.point1x, p_temp.point1y, p_temp.point2x,
-					p_temp.point2y);
+		if (/*pop_stack_pos_line*/line_store_pop(&p_temp.point1,&p_temp.point2)) {
+			push_stack_pos_line(nextline.point1, nextline.point2);
+			push_stack_pos_line(p_temp.point1, p_temp.point2);
 		}
 		track_state = GET_LINE_FROM_STACK; //weiterfahren mit der naechsten gueltigen Strecke
 
@@ -854,20 +955,19 @@
 
 		//falls weit gefahren wurde und Hindernis zum naechsten Zielpunkt erkannt wird aber der andere Streckenpunkt freie Fahrt bietet, dann einfach
 		//Punkte tauschen und den anderen Punkt zuerst anfahren um sich diesem von der anderen Seite zu naehern
-		if (go_long_distance && ((check_haz_in_map()) || map_get_field(
-				nextline.point1x, nextline.point1y) < 0) && map_way_free(x_pos,
-				y_pos, nextline.point2x, nextline.point2y))
-			set_nextline(0, 0, 0, 0, True); //Werte egal da nextline nur getauscht
+		if (go_long_distance && ((check_haz_sensDist()) || map_get_field(nextline.point1) < 0) && map_way_free(x_pos,
+				y_pos, nextline.point2.x, nextline.point2.y))
+			set_nextline(nextline.point1,nextline.point2, True); //Werte nur getauscht
 
 		//Ersten Streckenpunkt nun anfahren
-		bot_goto_pos(data, nextline.point1x, nextline.point1y, 999);
+		bot_goto_pos(data, nextline.point1.x, nextline.point1.y, 999);
 
 		lastCorrectionTime = 0; //Ablaufzeit init.
 
 
 		//bei kleinem Abstand nur Abgrund ueberwachen sonst ebenfalls auch Hindernisse mit Sensoren
 		if (go_long_distance)
-			bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_haz_in_map); // Abgrund und Abstandsensorcheck bei langer Entfernung (nach Stackholen oder rueckwaerts wg. zu nah)
+			bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_haz_sensDist); // Abgrund und Abstandsensorcheck bei langer Entfernung (nach Stackholen oder rueckwaerts wg. zu nah)
 		else
 			bot_cancel_behaviour(data, bot_goto_pos_behaviour,
 					check_border_fired); // kurzer Abstand nur Abgrundcheck oder sehr nah
@@ -886,10 +986,11 @@
 		BLOCK_BEHAVIOUR(data, 500); // etwas warten
 
 		//Ausrichten auf den Endpunkt der Fahrstrecke; ebenfalls wieder zum Aktualisieren der Map vor dem Anfahren selbst
-		bot_turn(data, calc_angle_diff(nextline.point2x - x_pos,nextline.point2y - y_pos));
+		bot_turn(data, calc_angle_diff(nextline.point2.x - x_pos,nextline.point2.y - y_pos));
 
 		//naechster Verhaltenszustand, welches wiederum der Starteintritt des Verhaltens selbst ist und geht hiermit von vorn los
 		track_state = CHECK_TRACKSIDE;
+		LOG_DEBUG("Ausrichten auf %1d %1d",nextline.point2.x,nextline.point2.y);
 
 		break;
 
@@ -900,40 +1001,52 @@
         //etwas hier verbleiben, damit Observer sicher gestoppt sind
 		if (!timer_ms_passed(&lastCheckTime, CORRECTION_DELAY + 300))
 			break;
-
+        
 		// auf jeden Fall hier Stop der Obserververhalten
 		if (!endrequest)
 		  bot_stop_observe();
 
 		// Weg vom Stack holen und Ende falls leer
-		if (!pop_stack_pos_line(&nextline.point1x, &nextline.point1y, &nextline.point2x, &nextline.point2y)) {
+		if (!line_store_pop(&nextline.point1, &nextline.point2)) {
 			LOG_DEBUG("Stack leer");
 			track_state = TRACK_END;
 			break;
 		}
-
+               
 		// der naheste Punkt zum Bot laut Luftdistance wird nextline
-		set_nextline(nextline.point1x, nextline.point1y, nextline.point2x, nextline.point2y, 0);
+		set_nextline(nextline.point1, nextline.point2, 0);
+		
+		LOG_DEBUG("Stackholen P1: %1d %1d P2: %1d %1d",nextline.point1.x,nextline.point1.y,nextline.point2.x,nextline.point2.y);
 
 		//pruefen, ob Wege zu den Punkten frei sind und Freikennungen speichern
-		free1 = map_way_free(x_pos, y_pos, nextline.point1x, nextline.point1y);
-		free2 = map_way_free(x_pos, y_pos, nextline.point2x, nextline.point2y);
+		uint8_t free1 = map_way_free(x_pos, y_pos, nextline.point1.x, nextline.point1.y);
+		
+		// bei Pfadplanung Weg um Hindernis finden und fahren; problematisch, weil auch ueber noch unbefahrene Gebiete geplant und gefahren wird
+		// ToDo: Pfadplanung ueber nur bereits befahrene Gebiete
+        #ifdef GO_WITH_PATHPLANING
+          if (!free1) {
+          	  bot_stop_observe();  // auf jeden Fall erst mal stoppen
+          	  LOG_DEBUG("Weg nicht frei, Pfadplanung zu %1d %1d",nextline.point1.x,nextline.point1.y);
+        	  bot_calc_wave(data,nextline.point1.x,nextline.point1.y); 
+          }
+        #else
+		
+		 uint8_t free2 = map_way_free(x_pos, y_pos, nextline.point2.x, nextline.point2.y);
 
 
-        // falls Weg zu keinem Punkt frei, dann mit dem naechsten Stackweg tauschen; ist dieser auch nicht befahrbar,
-        // wird dieser verworfen und der Weg
-        if (!free1 && !free2) {
+         // falls Weg zu keinem Punkt frei, dann mit dem naechsten Stackweg tauschen; ist dieser auch nicht befahrbar,
+         // wird dieser verworfen und der Weg
+         if (!free1 && !free2) {
 
           LOG_DEBUG("Stackweg nicht frei");
 
           static trackpoint_t pt;  //zum Merken beim Vertauschen
 
-		  if (pop_stack_pos_line(&pt.point1x, &pt.point1y, &pt.point2x, &pt.point2y)) {
+		  if (line_store_pop(&pt.point1, &pt.point2)) {
 
+		    free1 = map_way_free(x_pos, y_pos, pt.point1.x, pt.point1.y);
+		    free2 = map_way_free(x_pos, y_pos, pt.point2.x, pt.point2.y);
 
-		    free1 = map_way_free(x_pos, y_pos, pt.point1x, pt.point1y);
-		    free2 = map_way_free(x_pos, y_pos, pt.point2x, pt.point2y);
-
             if (!free1 && !free2) {
         	  LOG_DEBUG("Folgeweg auch nicht frei");  //Weg verworfen wenn nicht anfahrbar->naechsten Stackweg holen
               break;
@@ -941,13 +1054,15 @@
 
 		 	//falls naechster Stackweg auch nicht frei, dann weiter Stackwege holen bis zum Ende
 		 	//hier ist aber der Folgeweg anfahrbar und wird getauscht mit vorher geholtem, noch nicht anfahrbarem Weg
-		 	push_stack_pos_line(nextline.point1x, nextline.point1y, nextline.point2x, nextline.point2y);	//wieder auf Stack
-		 	push_stack_pos_line(pt.point1x, pt.point1y, pt.point2x, pt.point2y);	//wieder auf Stack
+		 	push_stack_pos_line(nextline.point1, nextline.point2);	//wieder auf Stack
+		 	push_stack_pos_line(pt.point1, pt.point2);	//wieder auf Stack
 		 	LOG_DEBUG("Weg mit Folgeweg getauscht");
 		 	break;
 		   }
 
-        }
+         }
+        #endif  
+        
 		//naechster Verhaltenszustand zum Anfahren der naechsten Bahn mit Ausrichten auf den naeheren Punkt laut nextline
 		track_state = TURN_TO_NEAREST;
 		break;
@@ -955,7 +1070,6 @@
 	default:
 		// am Ende des Verhaltens Observer stoppen
 		bot_stop_observe();
-		pos_store_clear(); // Stack bereinigen
 		return_from_behaviour(data);
 		break;
 	}
@@ -971,7 +1085,10 @@
 	switch_to_behaviour(caller, bot_drive_area_behaviour, OVERRIDE);
 	track_state = CHECK_TRACKSIDE;
 	border_fired = False;
-	pos_store_clear();
+	line_store_clear();  // Stack initialisieren
+    // naechste anzufahrende naechste Strecke erst mal initialisieren  
+    set_point_to_lastpoint(&nextline.point2, 0, 0);
+	set_point_to_lastpoint(&nextline.point1, 0, 0);
 
 	/* Kollisions-Verhalten ausschalten  */
 #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
Index: C:/eclipse/workspace/ct-Bot/Changelog.txt
===================================================================
--- C:/eclipse/workspace/ct-Bot/Changelog.txt	(revision 1498)
+++ C:/eclipse/workspace/ct-Bot/Changelog.txt	(working copy)
@@ -1,5 +1,7 @@
 CHANGELOG fuer c't-Bot
 ======================
+2008-10-27 Frank Menzel [Menzelfr@xxxxxxx]: Einbau des Pfadplanungsverhaltens in drive_area 
+
 2008-10-24 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Positionsspeicher um pos_store_get_sp(), pos_store_get_fp() und pos_store_pop_until() ergaenzt. Ermoeglicht pop bis zu einem gegebenen Stackpointer. 
 
 2008-10-24 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Bot-2-Bot-Kommunikation gegen ungueltige Kommandos abgesichert, Debug-Ausgaben ausgeschaltet
Index: C:/eclipse/workspace/ct-Bot/ct-Bot.h
===================================================================
--- C:/eclipse/workspace/ct-Bot/ct-Bot.h	(revision 1498)
+++ C:/eclipse/workspace/ct-Bot/ct-Bot.h	(working copy)
@@ -53,7 +53,7 @@
 #define MEASURE_MOUSE_AVAILABLE			/*!< Geschwindigkeiten werden aus den Maussensordaten berechnet */
 //#define MEASURE_COUPLED_AVAILABLE		/*!< Geschwindigkeiten werden aus Maus- und Encoderwerten ermittelt und gekoppelt */
 
-//#define POS_STORE_AVAILABLE  /*!< Positionsspeicher vorhanden */
+#define POS_STORE_AVAILABLE  /*!< Positionsspeicher vorhanden */
 
 //#define WELCOME_AVAILABLE	/*!< kleiner Willkommensgruss */
 
@@ -70,7 +70,7 @@
 
 #define BEHAVIOUR_AVAILABLE /*!< Nur wenn dieser Parameter gesetzt ist, exisitiert das Verhaltenssystem */
 
-//#define MAP_AVAILABLE /*!< Aktiviere die Kartographie */
+#define MAP_AVAILABLE /*!< Aktiviere die Kartographie */
 
 //#define SPEED_CONTROL_AVAILABLE /*!< Aktiviert die Motorregelung */
 //#define ADJUST_PID_PARAMS		/*!< macht PID-Paramter zur Laufzeit per FB einstellbar */
Index: C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h
===================================================================
--- C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h	(revision 1498)
+++ C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h	(working copy)
@@ -55,7 +55,7 @@
 
 #define BEHAVIOUR_DELAY_AVAILABLE /*!< Delay-Routinen als Verhalten */
 
-//#define BEHAVIOUR_DRIVE_AREA_AVAILABLE /*!< flaechendeckendes Fahren mit Map */
+#define BEHAVIOUR_DRIVE_AREA_AVAILABLE /*!< flaechendeckendes Fahren mit Map */
 
 /* Aufgrund einer ganzen reihe von Abhaengigkeiten sollte man beim Versuch Speicher
  * zu sparen, zuerst mal bei den Hauptverhalten ausmisten, sonst kommen die
Index: C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.core.prefs
===================================================================
--- C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.core.prefs	(revision 0)
+++ C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.core.prefs	(revision 0)
@@ -0,0 +1,3 @@
+#Mon Oct 27 16:31:49 CET 2008
+eclipse.preferences.version=1
+indexerId=org.eclipse.cdt.core.fastIndexer