- heise online
- c't Magazin
- iX Magazin
- Technology Review
- Telepolis

- heise mobil
- heise Security
- heise Netze
- Open Source
- heise Developer

- heise Resale
- heise Foto
- heise Autos
- c't-TV

- Software/Download
- Stellenangebote
- Preisvergleich
- IT-Markt

- heisetreff
- heise Veranstaltungen
- heise Whitepapers
- Abo&Heft
- Archiv

####

####

c't Projekte - c't-Bot und c't-Sim - Mailinglisten

## c't-Bot und c't-Sim

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

### AW: [ct-bot] Aenderungen im Pfadplanungsverhalten->area-Verhalten

Absender: Frank Menzel
Datum: Sa, 16.05.2009 12:13:03
In-reply-to: <000001c9d600$79fa4a40$0200a8c0@mexpnew>

    Sorry, war der falsche Patch, hier kommt der richtige Patch dazu...

    Gruß, Frank

    -----Ursprüngliche Nachricht-----
    Von: ct-bot-entwickler-bounces@xxxxxxxxxxxxxxxxx
    [mailto:ct-bot-entwickler-bounces@xxxxxxxxxxxxxxxxx] Im Auftrag von
    Frank Menzel
    Gesendet: Samstag, 16. Mai 2009 10:30
    An: 'Entwicklung rund um den c't-bot'
    Betreff: AW: [ct-bot] Aenderungen im
    Pfadplanungsverhalten->area-Verhalten

    Hallo Timo,
    anbei mal eine neue Version für das area-Verhalten, welches so im Zuge
    der Experimente mit der Pfadplanung entstanden ist und meiner Meinung
    nach in dieser Version besser durch die Welt kommt. Neben einigen
    weiteren Debugausgaben ist eine große Änderung die, dass er beim
    Anfahren des Zielpunktes und ausrichten auf diesem nicht sofort via
    obstacle losfährt und weiter geradeaus wenn frei, sondern erst bis zu
    diesem Punkt, um dann alle Checks für die weitere Fahrt voraus
    durchzuführen. So erkennt er z.B., dass voraus die Strecke schon
    befahren ist (testbots_home) und nicht weiter befahren werden muss.
    Gruß, Frank

    -----Ursprüngliche Nachricht-----
    Von: ct-bot-entwickler-bounces@xxxxxxxxxxxxxxxxx
    [mailto:ct-bot-entwickler-bounces@xxxxxxxxxxxxxxxxx] Im Auftrag von Timo
    Sandmann
    Gesendet: Donnerstag, 14. Mai 2009 20:37
    An: Entwicklung rund um den c't-bot
    Betreff: Re: [ct-bot] Aenderungen im Pfadplanungsverhalten

    -----BEGIN PGP SIGNED MESSAGE-----
    Hash: SHA1

    Hi Frank,

    Am 14.05.2009 um 17:18 schrieb Frank Menzel:
    > Hi Timo,
    > wenn's nicht gefällt, dann eben nicht:-(

    das Problem ist einfach: Mit dem Patch wird es in manchen Fällen
    besser und in manchen schlechter. Sobald als Vergleichswert nicht 0
    übergeben wird (evtl. möchte man dort ja auch Werte < 0 verwenden),
    werden die Hindernisse nicht mehr sauber erkannt. Ich denke, dass wir
    da langfristig eine Lösung brauchen, die in beiden Fällen ohne
    Nebenwirkungen funktioniert. Wir können aber erstmal die Änderung aus
    dem Patch per #define aus- und einschaltbar machen, je nachdem, ob man
    mehr Wert auf eine korrekte Hinderniserkennung oder auf die korrekte
    Erkennung des befahrenen Gebiets legt.

    > Die andere Änderung muss aber rein.

    Ja, kommt sie auch.

    Grüße,
    Timo

    -----BEGIN PGP SIGNATURE-----
    Version: GnuPG v1.4.9 (Darwin)

    iEYEARECAAYFAkoMZNIACgkQDH/BX4067fIUlwCfSpAwBsIeKIQN07MNJP/wetvn
    GNYAoOTnAy96VQ1F7N1bcKLW5wO8h6C0
    =8kPd
    -----END PGP SIGNATURE-----

    _______________________________________________
    ct-bot-entwickler Mailingliste
    ct-bot-entwickler@xxxxxxxxxxxxxxxxx
    http://www.heise.de/bin/newsletter/listinfo/ct-bot-entwickler

    Index: C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_pathplaning.c
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_pathplaning.c	(revision 1585)
    +++ C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_pathplaning.c	(working copy)
    @@ -50,7 +50,7 @@
     #include "display.h"
     #include "log.h"
     #include "pos_store.h"
    -
    +
     //#define DEBUG_PATHPLANING	// Schalter fuer Debugausgaben
     //#define DEBUG_PATHPLANING_VERBOSE	// zeichnet Zellen in die Map-Anzeige des Sim ein, rot: Hindernis, gruen: frei

    @@ -85,6 +85,7 @@
     #define MAP_CELL_SIZE_LOWRES	((uint16_t)(1000 / MAP_RESOLUTION_LOWRES))	/*!< Breite eines Map-Feldes in mm */
     #define MAP_LENGTH_LOWRES		((uint16_t)(MAP_SIZE_LOWRES * MAP_RESOLUTION_LOWRES))	/*!< Kantenlaenge der gesamten Karte in Map-Punkten */
     #define RATIO_THRESHOLD			(MAP_RATIO_FULL - 5)	/*!< Schwellwert, unterhalb dem das Ergebnis von map_get_ratio() als Hindernis gilt */
    +#define RATIO_THRESHOLD_DRIVEN  110     /*!< bei Pfadsuche auf befahrener Strecke gilt dieser Schwellwert   */

     /*! Anzahl der Sections in der Lowres-Map */
     #define MAP_SECTIONS_LOWRES (((uint16_t)(MAP_SIZE_LOWRES * MAP_RESOLUTION_LOWRES) / MAP_SECTION_POINTS_LOWRES))
    @@ -246,7 +247,8 @@
     #endif	// DEBUG_PATHPLANING_VERBOSE
     			uint8_t ratio = map_get_ratio(x1, yw, x2, yw, MAP_CELL_SIZE_LOWRES,
     					map_compare_haz, 127);
    -			if (ratio < RATIO_THRESHOLD) {
    +//fuer Vergleich auf bereits befahrene Strecke gilt anderer Schwellwert als fuer Hindernis
    +			if (ratio < ((map_compare_haz==0)?RATIO_THRESHOLD:RATIO_THRESHOLD_DRIVEN)) {
     				access_field_lowres((position_t) {x, y}, 1, 1);
     #if defined DEBUG_PATHPLANING_VERBOSE && defined MAP_2_SIM_AVAILABLE
     //				LOG_DEBUG("Trage Hindernis in (%d|%d) ein, ratio=%u", x, y, ratio);
    @@ -434,6 +436,8 @@
     				neighbour_found = False;
     			} else {
     				// abarbeiten aller 4 Nachbarn zu einem Punkt der Queue, auch wenn schon gefunden wurde zwecks guter Pfadfindung; ein True wird nicht ueberschrieben
    +				// nur 4 Nachbarn kann bei Pfadsuche auf befahrenem Gebiet zu keinem Pfad fuehren, wenn Bot bisher nur eine Fahrspur hat und schraeg faehrt, dann wuerde
    +				// nur 8er Nachbarschaft Pfad erkennen
     				for (j=-1; j<=1; j++) {
     					for (i=-1; i<=1; i++) {
     						if ((j == 0 && i != 0) || (i == 0 && j != 0)) {
    @@ -505,6 +509,14 @@
     						tmp.x += i;
     						tmp.y += j;
     						minval = access_field_lowres(tmp, 0, 0); // Mapwert auslesen
    +
    +						// Mapwert ist zur Zielerkennung egal, sind die Koords erreicht ist Schluss; wegen Mapwert 1 wurde manchmal oft kein Ziel erkannt
    +						// wenn ein Nachbar Zielpunkt ist, wird Endekennung gesetzt egal wie der Mapwert aussieht
    +						if (tmp.x == startwave.x && tmp.y == startwave.y) {
    +							endreached = True;
    +							LOG_DEBUG("Ende gefunden %1d %1d map: %1d", tmp.x, tmp.y, minval);
    +						}
    +
     						// die Koordinate mit niedrigstem Wellenwert merken
     						if (minval> 1 && minval < mapval_min) { // naechster genommener Wellenwert muss kleiner aus letztem Lauf sein
     							nextdest.x = pos.x + i;
    @@ -515,12 +527,6 @@
     							// Wellenwert und Kennung fuer Nachbar gefunden setzen
     							mapval_min = minval;
     							neighbour_found = True;
    -
    -							// wenn ein Nachbar Zielpunkt ist Endekennung setzen
    -							if (nextdest.x == startwave.x && nextdest.y == startwave.y) {
    -								endreached = True;
    -								LOG_DEBUG("Ende gefunden %1d %1d", nextdest.x, nextdest.y);
    -							}
     						}
     					}
     				}
    @@ -698,6 +704,11 @@
     		RC5_Code = 0;
     		show_labmap();
     		break;
    +
    +	case RC5_CODE_7:
    +		RC5_Code = 0;
    +		set_hazards();
    +		break;
     #endif
     	case RC5_CODE_8:
     		RC5_Code = 0;
    @@ -716,7 +727,11 @@
     	display_cursor(2, 1);
     	display_printf("4:PlanOnDrivenArea"); // nur auf befahrenem Gebiet planen
     	display_cursor(3, 1);
    +#ifdef DEBUG_PATHPLANING
    +    display_printf("5/7:GoPlaning/Haz");
    +#else
     	display_printf("5:GoPlaning");
    +#endif
     	display_cursor(4, 1);
     #ifdef DEBUG_PATHPLANING
     	display_printf("6/8:ShowMap/SetDest");
    Index: C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c	(revision 1585)
    +++ C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c	(working copy)
    @@ -46,7 +46,7 @@

     #define GO_WITH_PATHPLANING  		// auskommentieren, falls ohne Pfadplanung

    -//#define DEBUG_BEHAVIOUR_AREA		// Schalter fuer Debug-Code
    +#define DEBUG_BEHAVIOUR_AREA		// Schalter fuer Debug-Code
     //#define DEBUG_DRIVE_AREA_TIMES	// Schalter fuer Zeitmessungen

    @@ -72,7 +72,7 @@
     /*!  In diesem Abstand voraus [mm] ab Abstandssensoren wird auf Hindernis gecheckt */
     #define DIST_AWARD_HAZ  OPTIMAL_DISTANCE

    -#define MAP_WAY_FREE_MARGIN	10	/*!< Toleranzbereich links und rechts vom Bot fuer map_way_free() [mm] */
    +#define MAP_WAY_FREE_MARGIN     10      /*!< Toleranzbereich links und rechts vom Bot fuer map_way_free() [mm] */

     /*! Mindestabstand der Punkte fuer eine gueltige Bahn, welche es zu befahren gilt */
     #define DIST_VALID_QUAD (uint32_t)(140 * 140)
    @@ -133,22 +133,7 @@
      * ===== Start der allgemein fuer diese Verhalten notwendigen Routinen ==============
      * ==================================================================================*/

    -
    -#if defined MAP_2_SIM_AVAILABLE && defined DEBUG_BEHAVIOUR_AREA
     /*!
    - * Zeichnet eine Linie von Koordinate from nach to in der Farbe color in die Map ein;
    - * dient zur Visualisierung der Arbeitsweise des Verhaltens
    - * @param from	Koordinaten des ersten Punktes der Linie
    - * @param to	Koordinaten des zweiten Punktes der Linie
    - * @param color Farbe der Linie: 0=gruen, 1=rot, sonst schwarz
    - */
    -static void draw_line_world(position_t from, position_t to, uint8_t color) {
    -	command_write(CMD_MAP, SUB_MAP_CLEAR_LINES, 2, 0, 0);
    -	map_draw_line_world(from, to, color);
    -}
    -#endif	// MAP_2_SIM_AVAILABLE
    -
    -/*!
      * Notfallhandler, ausgefuehrt bei Abgrunderkennung und muss registriert werden
      */
     void border_drive_area_handler(void) {
    @@ -177,18 +162,20 @@
      * @param point2	Koordinaten des zweiten Punktes der Linie
      */
     static void push_stack_pos_line(position_t point1, position_t point2) {
    +
     	if ((point1.x == 0 && point1.y == 0) || (point2.x == 0 && point2.y == 0))
     		return; // ungueltige Werte werden nicht uebernommen

     	// Push der Linien-Einzelpunkte
     	pos_store_push(pos_store, point1);
     	pos_store_push(pos_store, point2);
    -
    +
     	LOG_DEBUG("PUSH x1y1: %1d %1d x2y2: %1d %1d", point1.x,point1.y,point2.x,point2.y);
    -
    -#if defined MAP_2_SIM_AVAILABLE && defined DEBUG_BEHAVIOUR_AREA
    -	draw_line_world(point1, point2, 2);  // schwarze Linie
    +
    +#ifdef MAP_2_SIM_AVAILABLE
    +		map_draw_line_world(point1,point2,2);  // schwarze Linie
     #endif
    +
     }

     /*!
    @@ -354,46 +341,45 @@
     	position_t map_pos;
     	int8_t mapval = 0;

    -	if (map_locked() == 1) {
    +	if (map_locked() == 1)
     		return False;
    -	}

    +
     	// nur wenn bot seit letztem Zugriff gewisse Strecke gefahren ist erfolgt Mapzugriff
         uint8_t mapaccess = (checkside == TRACKLEFT) ? check_map_after_distance(&observe_lastpos.point1.x, &observe_lastpos.point1.y) :
         	check_map_after_distance(&observe_lastpos.point2.x, &observe_lastpos.point2.y);

     	// weitere Checks nur wenn gewisse Strecke gefahren oder Endeanforderung besteht
    -    if (!endrequest && !mapaccess) {
    -		return False;  // Endepunktsuche ungueltig
    -    }
    +    if (!endrequest && !mapaccess)
    +		return False;  //Endepunktsuche ungueltig

     	// hier seitenabhaengig auf die jeweilige Spur sehen, Mapwert holen und die Koordinaten des Punktes der Nebenbahn
     	mapval = getpoint_side_dist(BOT_DIAMETER - SIDEDIST_MINUS, 10, checkside,//etwas voraus
     			&map_pos);

    -	// Endepunktsuche nicht weiter verfolgen falls noch kein Startpunkt vorliegt
    +	//Endepunktsuche nicht weiter verfolgen falls noch kein Startpunkt vorliegt
     	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
    +    //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) && &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(lastpoint, 0, 0);
    -		return False; //Endepunktsuche ungueltig
    -	}
    +			set_point_to_lastpoint(&observer->point1, 0, 0);
    +            *behavstate = (endrequest) ? TRACK_END : 0;// weiter mit Startpunktsuche
    +            set_point_to_lastpoint(lastpoint, 0, 0);
    +			return False; //Endepunktsuche ungueltig
    +		}

         // Abstand zum Startpunkt muss gueltig sein sonst ist Schluss
     	if (!(dist_valid(observer->point1, map_pos))) {
    -		*behavstate = (endrequest) ? TRACK_END : 1;// bleibt in Endepunktsuche wenn nicht Endeanforderung kam
    -		if (endrequest) {
    -		  LOG_DEBUG("Nebenpunkt zum Startpunkt dist nicht valid %1.0f", sqrt(get_dist(observer->point1.x,observer->point1.y,map_pos.x,map_pos.y)));
    +			*behavstate = (endrequest) ? TRACK_END : 1;// bleibt in Endepunktsuche wenn nicht Endeanforderung kam
    +			if (endrequest) {
    +			  LOG_DEBUG("Nebenpunkt zum Startpunkt dist nicht valid %1.0f", sqrt(get_dist(observer->point1.x,observer->point1.y,map_pos.x,map_pos.y)));
    +			}
    +			return False; //Endepunktsuche ungueltig
     		}
    -		return False; // Endepunktsuche ungueltig
    -	}

         // hier wird nun unterschieden ob Nebenspur schon befahren ist oder sich dort Hindernis befindet; in diesem Fall wird sich
         // die Strecke als gueltig bis zu dem Punkt vor dem Hindernis auf dem Stack gemerkt; weiter dann mit Startpunktsuche
    @@ -403,18 +389,17 @@
     		// vorher auf Gueltigkeit gecheckt wird, d.h. muss bestimmten Mindestabstand zum Startpunkt haben und kein Hindernis sein
     		set_point_to_lastpoint(&observer->point2, lastpoint->x, lastpoint->y);

    -		// zuletzt gueltigen Endpunkt auf Stack sichern
    +		//zuletzt gueltigen Endpunkt auf Stack sichern
     		push_stack_pos_line(observer->point1,*lastpoint);
    +
    +		if (lastpoint->x!=0 || lastpoint->y!=0)
    +		  LOG_DEBUG("Autopush wg. Hind oder befahren xy: %1d %1d mapval: %1d",lastpoint->x,lastpoint->y,mapval);

    -		if (lastpoint->x != 0 || lastpoint->y != 0) {
    -			LOG_DEBUG("Autopush wg. Hind oder befahren xy: %1d %1d mapval: %1d",lastpoint->x,lastpoint->y,mapval);
    -		}
    -
    -		// letzten gemerkten Punkt wieder als ungueltig kennzeichnen
    +		//letzten gemerkten Punkt wieder als ungueltig kennzeichnen
     	    set_point_to_lastpoint(lastpoint, 0, 0);

     	} else {
    -		// Punkt auf Nebenbahn hat hier keine Hinderniswahrscheinlichkeit und ist noch nicht befahren
    +		//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(lastpoint, map_pos.x, map_pos.y);
    @@ -423,7 +408,7 @@
     		// und weiter mit Endpunktsuche
     		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
    +		    *behavstate = (endrequest) ? TRACK_END : 1;// bleibt in Endepunktsuche wenn nicht Endeanforderung kam

     			set_point_to_lastpoint(lastpoint, 0, 0);
     			set_point_to_lastpoint(&observer->point2, 0, 0);
    @@ -431,14 +416,13 @@
     		}

     		// bei Nicht Ende geht Endesuche weiter;bei Endeanforderung ist dies der aktuelle Endpunkt fuer den Stack
    -		if (!endrequest) { // weiter nur falls kein Ende kommen soll sonst ist dies nicht der echte Endpunkt
    +		if (!endrequest) // weiter nur falls kein Ende kommen soll sonst ist dies nicht der echte Endpunkt
     			return False;
    -		}

     		// gueltigen Endepunkt in Observervar vermerken
     		set_point_to_lastpoint(&observer->point2, lastpoint->x, lastpoint->y);

    -		// Merken der Strecke bei Endanforderung auf dem Stack
    +		//Merken der Strecke bei Endanforderung auf dem Stack
     		push_stack_pos_line(observer->point1,*lastpoint);

     	}
    @@ -450,6 +434,7 @@
     	set_point_to_lastpoint(&observer->point1, 0, 0);

     	return True; // Streckenpunkt ist hier gueltig
    +
     }

     /*! Merkvariable des letzten gueltigen Endpunktes fuer Obserververhalten linke Spur */
    @@ -468,9 +453,8 @@
     	switch (observe1_state) {
     	case OBSERVE_SEARCH_STARTPOINT:
     		// Eingang fuer Startpunktsuche
    -		if (!observe_get_startpoint(CHECK_SIDE, &observe_trackleft, &observe1_state)) {
    +		if (!observe_get_startpoint(CHECK_SIDE, &observe_trackleft, &observe1_state))
     			break;
    -		}

     		observe1_state = OBSERVE_SEARCH_ENDPOINT;
     		break;
    @@ -480,15 +464,13 @@
     		// 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)) {
    +				&observe_trackleft, &lastpointleft))
     			break;
    -		}

     		break;
     	default:
    -		if (observe_trackleft.point2.x == 0 && observe_trackleft.point2.y == 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;
    @@ -508,6 +490,7 @@
     		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);
    +
     	}
     }

    @@ -527,9 +510,8 @@
     	switch (observe2_state) {
     	case OBSERVE_SEARCH_STARTPOINT:
     		// Eingang fuer Startpunktsuche
    -		if (!observe_get_startpoint(CHECK_SIDE, &observe_trackright, &observe2_state)) {
    +		if (!observe_get_startpoint(CHECK_SIDE, &observe_trackright, &observe2_state))
     			break;
    -		}

     		observe2_state = OBSERVE_SEARCH_ENDPOINT;

    @@ -540,15 +522,13 @@
     		// 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)) {
    +				&observe_trackright, &lastpointright))
     			break;
    -		}

     		break;
     	default:
    -		if (observe_trackright.point2.x == 0 && observe_trackright.point2.y == 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;
    @@ -567,7 +547,9 @@
     		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);
    +
     	}
    +
     }

     /*!
    @@ -608,13 +590,11 @@
      * @return True wenn Hindernis voraus sonst False
      */
     static uint8_t check_haz_sensDist(void) {
    -	if (border_fired) {
    +	if (border_fired)
     		return True;
    -	}

    -	if (sensDistL <= DIST_AWARD_HAZ || sensDistR <= DIST_AWARD_HAZ) {
    +	if (sensDistL <= DIST_AWARD_HAZ || sensDistR <= DIST_AWARD_HAZ)
     		return True;
    -	}

     	return False;
     }
    @@ -639,6 +619,7 @@
     	} else {
     		// der Punkt in der Naehe wird richtig nach nextline uebernommen; der naheste nach Punkt1
     		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.point1, point1.x, point1.y);
     			set_point_to_lastpoint(&nextline.point2, point2.x, point2.y);

    @@ -685,56 +666,84 @@

     	switch (track_state) {

    -	case CHECK_TRACKSIDE: // Vorwaeertsfahren mit Start der Observer
    +	case CHECK_TRACKSIDE: //Vorwaeertsfahren mit Start der Observer

     		BLOCK_BEHAVIOUR(data, 500); // etwas warten

    -		if (check_haz_sensDist() || (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
     			track_state = GET_LINE_FROM_STACK;
     			LOG_DEBUG("zu Stackholen wg. Abstand %1d %1d", sensDistL, sensDistR);
    -
    -#if defined MAP_2_SIM_AVAILABLE && defined DEBUG_BEHAVIOUR_AREA
    -			if (nextline.point2.x != 0 || nextline.point2.y != 0) {
    -				draw_line_world(nextline.point1, nextline.point2, 1); // Strecke wird verworfen, nicht anfahrbar und rot darstellen
    -			}
    -#endif	// MAP_2_SIM_AVAILABLE
    -
    +
    +#ifdef MAP_2_SIM_AVAILABLE
    +	        if (nextline.point2.x != 0 || nextline.point2.y != 0)
    +		      map_draw_line_world(nextline.point1,nextline.point2,1);//Strecke wird verworfen, nicht anfahrbar und rot darstellen
    +#endif
    +
     			break;
     		}

     		//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
     		disttodrive = 900; //hoher init. Wert
    +		int8_t mapval=0;
     		if (sensDistL < 400 || sensDistR < 400) {
    -
    +
                 //notwendig, weil bei Map-Simexport Lampen als genausogrosses Hindernis gekennzeichnet sind wie Wandelemente, d.h. gesehener Abstand groesser
                 //als in Map eingetragen und gesehener Anfahrpunkt waere im Hindernis und Strecke nicht anfahrbar; daher so weit kleinere Strecke
                 //als gesehene nehmen, damit Punkt auf jeden Fall vor Hindernisfeld liegt
     			disttodrive = (sensDistL < sensDistR) ? sensDistL - 80 : sensDistR
     					- 80; //in kurzer Entfernung Hind. gesehen
    -			getpoint_side_dist(0, disttodrive, TRACKLEFT, &pos); // Punkt etwas vor Hindernis berechnen
    -			LOG_DEBUG("Mappunkt fuer %1d voraus berechnet l/r: %1d %1d", disttodrive, sensDistL, sensDistR);
    +			mapval=getpoint_side_dist(0, disttodrive, TRACKLEFT, &pos); // Punkt etwas vor Hindernis berechnen
    +			LOG_DEBUG("Map-Punkt %1d fuer %1d voraus berechnet l/r: %1d %1d",mapval,disttodrive,sensDistL, sensDistR);
     		} else {
    -			getpoint_side_dist(0,250, TRACKLEFT, &pos); // nur etwas voraus Punkt berechnen wenn weiter gesehen
    -			LOG_DEBUG("Mappunkt fuer 25cm voraus berechnet l/r: %1d %1d", sensDistL, sensDistR);
    +
    +            // falls bei Weitsicht voraus berechneter Streckenpunkt schon befahren ist, dann nur bis dorthin fahren und nicht via obstacle; danach geht's wieder von
    +            // vorn  mit geradeausfahren weiter und saemtlichen Pruefungen; Beispielparcour hierfuer ist testbots_home
    + 			mapval=getpoint_side_dist(0,800, TRACKLEFT, &pos); //nur etwas voraus Punkt berechnen wenn weiter gesehen
    +			LOG_DEBUG("Map-Punkt %1d 80cm voraus ",mapval);
    +
    +			if (mapval>MAP_TRACKVAL) {
    +			  LOG_DEBUG("Map-Punkt %1d 80cm voraus befahren l/r: %1d %1d",mapval,sensDistL, sensDistR);
    +			  disttodrive=400;  // bei befahren voraus stueckchenweise ranfahren
    +			}
    +			else {
    +				mapval=getpoint_side_dist(0,250, TRACKLEFT, &pos); //nur etwas voraus Punkt berechnen wenn weiter gesehen
    +				LOG_DEBUG("MapPunkt %1d fuer 25cm voraus: %1d",mapval);
    +			}
    +			// wenn Zielpunkt bekannt ist, dann nur Strecke bis Zielpunkt fahren und danach alle Plausis neu fuer moegliche Weiterfahrt
    +			if (nextline.point2.x!=0 || nextline.point2.y!=0) {
    +			 position_t bot_pos;
    +		     bot_pos.x=x_pos;
    +		     bot_pos.y=y_pos;
    +		     disttodrive=sqrt(get_dist(bot_pos.x,bot_pos.y,pos.x,pos.y));
    +		     LOG_DEBUG("Weitsicht und P2 bekannt-nur bestimmte Laenge fahren %1d",disttodrive);
    +			}
     		}
    -
    -		if (nextline.point2.x!=0 || nextline.point2.y!=0) {
    -		  position_t botpos;
    +
    +		if (nextline.point2.x!=0 || nextline.point2.y!=0) {
    +		  LOG_DEBUG("Mapwerte nextline P1: %1d P2: %1d",map_get_field(nextline.point1),map_get_field(nextline.point1));
    +		  position_t botpos;
     		    botpos.x=x_pos;
    -		    botpos.y=y_pos;
    +		    botpos.y=y_pos;
     		   // Wenn Tracklaenge kleiner als Laenge voraus laut Abstandssensoren, dann Tracklaenge nehmen
    -		   if (get_dist(nextline.point1.x, nextline.point1.y,
    -					nextline.point2.x, nextline.point2.y) < get_dist(botpos.x,
    -					botpos.y, pos.x, pos.y)) {
    -				getpoint_side_dist(0, sqrt(
    -						get_dist(nextline.point1.x, nextline.point1.y,
    -								nextline.point2.x, nextline.point2.y)),
    -						TRACKLEFT, &pos); //nur etwas voraus Punkt berechnen wenn weiter gesehen
    -				LOG_DEBUG("Mappunkt korrigiert fuer kleinere Tracklaenge %1.0f",
    -						sqrt(get_dist(nextline.point1.x, nextline.point1.y, nextline.point2.x, nextline.point2.y)));
    -			}
    +		   if (get_dist(nextline.point1.x,nextline.point1.y,nextline.point2.x,nextline.point2.y)< get_dist(botpos.x,botpos.y,pos.x,pos.y)) {
    +		   	 disttodrive=sqrt(get_dist(nextline.point1.x,nextline.point1.y,nextline.point2.x,nextline.point2.y));
    +		   	 getpoint_side_dist(0,disttodrive, TRACKLEFT, &pos); //nur etwas voraus Punkt berechnen wenn weiter gesehen
    +			 LOG_DEBUG("Mappunkt korrigiert fuer kleinere Tracklaenge %1d",disttodrive);
    +		   }
    +
     		}
    +		/*else { // freie Fahrt voraus ohne Zielpunkt
    +			//mapval=getpoint_side_dist(0,250, TRACKLEFT, &pos);  // nur im geringen Abstand voraus pruefen auf schon befahren
    +			LOG_DEBUG("-Mapwert- voraus: %1d",mapval);
    +			if (mapval > MAP_TRACKVAL) {
    +			  track_state = GET_LINE_FROM_STACK;
    +			  LOG_DEBUG("Spur voraus befahren->zu Stackholen");
    +			  break;
    +		    }
    +
    +		}*/
    +

     		// Wegfreiheit bis zum Punkt auf Strecke voraus bestimmen; manchmal kein Hind gesehen aber schon in Map
     		uint8_t free1 = map_way_free(x_pos, y_pos, pos.x, pos.y, MAP_WAY_FREE_MARGIN);
    @@ -748,12 +757,12 @@
     			// einem Schwellwert wirklich verwerfen
     			track_state = GET_LINE_FROM_STACK;
     			LOG_DEBUG("Weg versperrt zu Zwischenpunkt %1d %1d, Endpunkt %1d %1d", pos.x, pos.y, nextline.point2.x, nextline.point2.y);
    -#if defined MAP_2_SIM_AVAILABLE && defined DEBUG_BEHAVIOUR_AREA
    +#ifdef MAP_2_SIM_AVAILABLE
     		    position_t aktpos;
     		    aktpos.x=x_pos;
     		    aktpos.y=y_pos;
    -		    draw_line_world(aktpos, pos, 1); // Linie wird rot, so weit voraus ist Weg nicht frei
    -#endif	// MAP_2_SIM_AVAILABLE
    +		    map_draw_line_world(aktpos,pos,1);//Linie wird rot, so weit voraus ist Weg nicht frei
    +#endif

     			break;
     		}
    @@ -772,14 +781,20 @@
     		break;

     	case GO_FORWARD: //nach bestandenen Pruefungen fuer Bahn voraus nun vorwaertsfahren
    -		track_state = DELAY_AFTER_FORWARD; //naechster Verhaltenszustand
    +		track_state = DELAY_AFTER_FORWARD;    //naechster Verhaltenszustand
    +		next_behavstate=GET_LINE_FROM_STACK;  //Verhaltenszustand nach AFTER_FORWARD

     		//wenn bereits Hindernis voraus gesehen wurde, dann nur Fahren bis kurz vor dem Hindernis falls
     		//naemlich Hindernis wieder aus Blick der Sensoren verschwindet und nicht daran kleben bleibt
     		if (disttodrive < 900) {
     			bot_goto_dist(data, disttodrive, 1); //bei kleinem Abstand mit goto_dist fahren
    +			LOG_DEBUG("fahre disttodrive %1d",disttodrive);
    +			track_state=DELAY_AFTER_FORWARD;  // um Observer zu stoppen und Nebenbahnen zu pushen
    +			next_behavstate=CHECK_TRACKSIDE;  // mit voraus weiter nach AFTER_FORWARD und saemtlichen Checks voraus
    +			set_point_to_lastpoint(&nextline.point2, 0, 0);  // Zielpunkt ist dann bereits angefahren und einfach vorwaerts weiter
     		} else {
     			bot_goto_obstacle(data, DIST_AWARD_HAZ, 0); //mit hohem Abstand voraus via obstacle
    +			LOG_DEBUG("fahre obstacle ");
     		}

     		//damit auch Abgrund erkannt wird beim Vorfahren; obige Verhalten benutzen beide goto_pos
    @@ -791,12 +806,13 @@

     		//bei Abgrund etwas rueckwaerts fahren
     		if (border_fired) {
    -			bot_drive_distance(data, 0, -BOT_SPEED_NORMAL, OPTIMAL_DISTANCE / 10); // rueckwaerts bei Abgrund
    +			bot_drive_distance(data, 0, -BOT_SPEED_NORMAL, OPTIMAL_DISTANCE/10); // rueckwaerts bei Abgrund
     		}

     		lastCheckTime = TIMER_GET_TICKCOUNT_16; // Var auf Systemzeit setzen, damit etwas zeit vergehen kann im naechsten Zustand
     		track_state = AFTER_FORWARD; //naechster Verhaltenszustand
    -		next_behavstate=GET_LINE_FROM_STACK;  //Verhaltenszustand nach AFTER_FORWARD
    +		//next_behavstate=GET_LINE_FROM_STACK;  //Verhaltenszustand nach AFTER_FORWARD
    +

     		break;

    @@ -818,6 +834,16 @@

     		// naechster Verhaltenszustand abhaengig von gesetzter naechster Zustandsvariablen
     		track_state = next_behavstate;
    +
    +		// Falls es mit Weiterfahrt weitergehen soll, dann erst pruefen auf Befahren voraus
    +		if (track_state==CHECK_TRACKSIDE) {
    +			mapval=getpoint_side_dist(0,250, TRACKLEFT, &pos);
    +			LOG_DEBUG("AFTER_FORWARD Spur voraus %1d",mapval);
    +			if (mapval > MAP_TRACKVAL) {
    +			      track_state = GET_LINE_FROM_STACK;
    +			      LOG_DEBUG("Spur knapp voraus befahren->zu Stackholen");
    +		     }
    +		}

     		break;

    @@ -883,58 +909,60 @@
     			//damit bot nicht auf seinem weiten Weg zum Zielpunkt in den Abgrund faellt
     			bot_cancel_behaviour(data, bot_goto_pos_behaviour,
     					check_haz_sensDist);
    -		} else {
    -			// in Testparcours2 bleibt bot oft an unterer rechter Ecke haengen, weil der Weg als Frei erkannt wird obwohl auch laut
    -			// Map schon die Ecke Grau-Schwarz eingezeichnet ist (bei nicht uebergebener Karte mit on_the_fly Hindernis-Aktualisierung)
    -			// aber selbst mit uebergebener clean Karte aus sim-Export bleibt er haengen, da wap_way_free Weg frei liefert
    -			uint8_t free1 = map_way_free(x_pos, y_pos, nextline.point1.x, nextline.point1.y, MAP_WAY_FREE_MARGIN);
    -			LOG_DEBUG("kurze Wegefreiheit zu P1: %1d, l/r: %1d %1d, Zielabstand %1d", free1, sensDistL, sensDistR, dist_to_point);
    +		}
    +		else {
    +		  // in Testparcours2 bleibt bot oft an unterer rechter Ecke haengen, weil der Weg als Frei erkannt wird obwohl auch laut
    +		  // Map schon die Ecke Grau-Schwarz eingezeichnet ist (bei nicht uebergebener Karte mit on_the_fly Hindernis-Aktualisierung)
    +		  // aber selbst mit uebergebener clean Karte aus sim-Export bleibt er haengen, da wap_way_free Weg frei liefert
    +		  uint8_t free1 = map_way_free(x_pos, y_pos, nextline.point1.x,nextline.point1.y, MAP_WAY_FREE_MARGIN);
    +		  LOG_DEBUG("kurze Wegefreiheit zu P1: %1d, l/r: %1d %1d, Zielabstand %1d",free1,sensDistL,sensDistR,dist_to_point);
    +
    +		  // Falls Abstand des Zielpunktes kleiner als ein gesehener Hindernisabstand ist, dann auch nicht anfahren
    +          if (free1 && (dist_to_point>=sensDistL+20 || dist_to_point >=sensDistR+20)) {
    +        	  LOG_DEBUG("Abstand zu fahren groesser als Hindabstand l/r: %1d %1d",sensDistL,sensDistR);
    +        	  free1=False;
    +          }
    +
    +          if (!free1) {// Visualisierung der Nicht Anfahrbarkeit
    +#ifdef MAP_2_SIM_AVAILABLE  // zur Visualisierung Weg zu P1 Rot einfaerben
    +		position_t akt_pos;
    +		akt_pos.x=x_pos;
    +		akt_pos.y=y_pos;
    +		map_draw_line_world(akt_pos,nextline.point1,1);//Linie von Botpos zu P1 wird rot
    +#endif
    +          }
    +
    +
    +         // evtl erst den Punkt 2 anfahren wenn P1 nicht anfahrbar war, d.h. Punkte vertauschen
    +         if (!free1 /*&& (get_dist(nextline.point1.x, nextline.point1.y,nextline.point2.x, nextline.point2.y)>300*300)*/) {
    +          uint8_t free2 = map_way_free(x_pos, y_pos, nextline.point2.x, nextline.point2.y, MAP_WAY_FREE_MARGIN);
    + 	      LOG_DEBUG("Wegfreiheit P2: %1d, Abstand P1: %1.0f P2: %1.0f",free2, sqrt(get_dist(x_pos,y_pos,nextline.point1.x, nextline.point1.y)),sqrt(get_dist(x_pos,y_pos,nextline.point2.x, nextline.point2.y)));
    +          if (free2) {
    +           set_nextline(nextline.point1, nextline.point2, True); //Werte egal da nextline nur getauscht
    +           next_behavstate=TURN_TO_NEAREST;
    +           track_state=AFTER_FORWARD;//muss sich hiernach zum Zielpunkt ausrichten
    +           LOG_DEBUG("P2 anfahren, da dieser frei");
    +           break;
    +          }
    +         }
    +
    +        // ist der Weg voraus nicht frei laut Map in geringerem Abstand, dann gar nicht erst fahren und verwerfen
    +		if (!free1) {
    +			track_state = GET_LINE_FROM_STACK;
    +			LOG_DEBUG("P1 nicht anfahrbar %1d %1d", nextline.point1.x,nextline.point1.y);

    -			// Falls Abstand des Zielpunktes kleiner als ein gesehener Hindernisabstand ist, dann auch nicht anfahren
    -			if (free1 && (dist_to_point >= sensDistL + 20 || dist_to_point
    -					>= sensDistR + 20)) {
    -				LOG_DEBUG("Abstand zu fahren groesser als Hindabstand l/r: %1d %1d", sensDistL, sensDistR);
    -				free1 = False;
    -			}
    +#ifdef MAP_2_SIM_AVAILABLE  // zur Visualisierung Weg zu P1 Rot einfaerben
    +		position_t aktpos;
    +		aktpos.x=x_pos;
    +		aktpos.y=y_pos;
    +		map_draw_line_world(nextline.point1,nextline.point2,1);// eigentliche Fahrspur wird verworfen und auch Rot einfaerben
    +
    +#endif
    +        break;
    +		}
    +
    +		}

    -			if (!free1) { // Visualisierung der Nicht Anfahrbarkeit
    -#if defined MAP_2_SIM_AVAILABLE && defined DEBUG_BEHAVIOUR_AREA  // zur Visualisierung Weg zu P1 Rot einfaerben
    -				position_t akt_pos;
    -				akt_pos.x = x_pos;
    -				akt_pos.y = y_pos;
    -				draw_line_world(akt_pos, nextline.point1, 1);//Linie von Botpos zu P1 wird rot
    -#endif	// MAP_2_SIM_AVAILABLE
    -			}
    -
    -			// evtl erst den Punkt 2 anfahren wenn P1 nicht anfahrbar war, d.h. Punkte vertauschen
    -			if (!free1 /*&& (get_dist(nextline.point1.x, nextline.point1.y,nextline.point2.x, nextline.point2.y)>300*300)*/) {
    -				uint8_t free2 = map_way_free(x_pos, y_pos, nextline.point2.x,
    -						nextline.point2.y, MAP_WAY_FREE_MARGIN);
    -				LOG_DEBUG("Wegfreiheit P2: %1d, Abstand P1: %1.0f P2: %1.0f", free2, sqrt(get_dist(x_pos, y_pos, nextline.point1.x, nextline.point1.y)),
    -						sqrt(get_dist(x_pos,y_pos,nextline.point2.x, nextline.point2.y)));
    -				if (free2) {
    -					set_nextline(nextline.point1, nextline.point2, True); //Werte egal da nextline nur getauscht
    -					next_behavstate = TURN_TO_NEAREST;
    -					track_state = AFTER_FORWARD;//muss sich hiernach zum Zielpunkt ausrichten
    -					LOG_DEBUG("P2 anfahren, da dieser frei");
    -					break;
    -				}
    -			}
    -
    -			// ist der Weg voraus nicht frei laut Map in geringerem Abstand, dann gar nicht erst fahren und verwerfen
    -			if (!free1) {
    -				track_state = GET_LINE_FROM_STACK;
    -				LOG_DEBUG("P1 nicht anfahrbar %1d %1d", nextline.point1.x, nextline.point1.y);
    -
    -#if defined MAP_2_SIM_AVAILABLE && defined DEBUG_BEHAVIOUR_AREA // zur Visualisierung Weg zu P1 Rot einfaerben
    -				position_t aktpos;
    -				aktpos.x = x_pos;
    -				aktpos.y = y_pos;
    -				draw_line_world(nextline.point1, nextline.point2, 1); // eigentliche Fahrspur wird verworfen und auch Rot einfaerben
    -#endif	// MAP_2_SIM_AVAILABLE
    -				break;
    -			}
    -		}
     		break;

     	case CHECK_DIST:
    @@ -984,10 +1012,10 @@
     		//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_sensDist); // Abgrund und Abstandsensorcheck bei langer Entfernung (nach Stackholen oder rueckwaerts wg. zu nah)
    +					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
    +					check_border_fired); // kurzer Abstand nur Abgrundcheck oder sehr nah
     		}

     		break;
    @@ -1014,10 +1042,10 @@
     		break;

     	case GET_LINE_FROM_STACK:
    -		// kommt hierher, wenn  die naechste Fahrstrecke zu holen ist; kann sowohl die Bahn daneben sein als auch eine weit
    -		// entfernte Strecke, die ehemals eine Wegalternative war
    +		//kommt hierher, wenn  die naechste Fahrstrecke zu holen ist; kann sowohl die Bahn daneben sein als auch eine weit
    +		//entfernte Strecke, die ehemals eine Wegalternative war

    -		// etwas hier verbleiben, damit Observer sicher gestoppt sind
    +		//etwas hier verbleiben, damit Observer sicher gestoppt sind
     		if (!timer_ms_passed_32(&lastCheckTime, CORRECTION_DELAY + 300)) {
     			break;
     		}
    @@ -1037,74 +1065,107 @@
     		// der naheste Punkt zum Bot laut Luftdistance wird nextline
     		set_nextline(nextline.point1, nextline.point2, 0);

    -#if defined MAP_2_SIM_AVAILABLE && defined DEBUG_BEHAVIOUR_AREA
    -		draw_line_world(nextline.point1, nextline.point2, 0); // naechste anzufahrende Linie gruen
    +		LOG_DEBUG("Stackholen P1: %1d %1d P2: %1d %1d Mapwerte: %1d %1d", nextline.point1.x, nextline.point1.y, nextline.point2.x, nextline.point2.y,
    +		           map_get_field(nextline.point1),map_get_field(nextline.point2));
    +
    +
    +#ifdef MAP_2_SIM_AVAILABLE
    +		map_draw_line_world(nextline.point1,nextline.point2,0);//naechste anzufahrende Linie gruen
     #endif

    -		LOG_DEBUG("Stackholen P1: %1d %1d P2: %1d %1d", nextline.point1.x, nextline.point1.y, nextline.point2.x, nextline.point2.y);
    +		if (map_get_field(nextline.point1)>MAP_TRACKVAL || map_get_field(nextline.point2)>MAP_TRACKVAL) {
    +			LOG_DEBUG("Start- oder EndPunkt schon befahren->Track verwerfen");
    +#ifdef MAP_2_SIM_AVAILABLE
    +		map_draw_line_world(nextline.point1,nextline.point2,1);//verworfene Linie Rot
    +#endif
    +		  break;
    +		}

     		//pruefen, ob Wege zu den Punkten frei sind und Freikennungen speichern
     		free1 = map_way_free(x_pos, y_pos, nextline.point1.x, nextline.point1.y, MAP_WAY_FREE_MARGIN);
    -	    LOG_DEBUG("Wegfreiheit zu P1: %1d, Abstand P1: %1.0f P2: %1.0f",free1, sqrt(get_dist(x_pos,y_pos,nextline.point1.x, nextline.point1.y)),sqrt(get_dist(x_pos,y_pos,nextline.point2.x, nextline.point2.y)));
    +	    LOG_DEBUG("Wegfreiheit zu P1: %1d, Abstand P1: %1.0f P2: %1.0f, Mapwert: %1d",free1, sqrt(get_dist(x_pos,y_pos,nextline.point1.x, nextline.point1.y)),sqrt(get_dist(x_pos,y_pos,nextline.point2.x, nextline.point2.y)),map_get_field(nextline.point1));

    -       // wenn Weg zu P1 nicht frei ist aber Weg zu P2 und Abstand nicht zu kurz ist, dann zuerst P2 anfahren
    +       // wenn Weg zu P1 nicht frei ist aber Weg zu P2 und Abstand nicht zu kurz ist, dann zuerst P2 anfahren
            if (!free1) {
    -			uint8_t free2 = map_way_free(x_pos, y_pos, nextline.point2.x,
    -					nextline.point2.y, MAP_WAY_FREE_MARGIN);
    -			LOG_DEBUG("Wegfreiheit zu P2: %1d, Abstand P1: %1.0f P2: %1.0f",free2, sqrt(get_dist(x_pos,y_pos,nextline.point1.x, nextline.point1.y)),sqrt(get_dist(x_pos,y_pos,nextline.point2.x, nextline.point2.y)));
    -			if (free2 && (get_dist(nextline.point2.x, nextline.point2.y, x_pos,
    -					y_pos) >= 300* 300 )) {
    -				set_nextline(nextline.point1, nextline.point2, True); //Werte egal da nextline nur getauscht
    -				track_state = AFTER_FORWARD;//muss sich hiernach zum Zielpunkt ausrichten
    -				next_behavstate = TURN_TO_NEAREST; //Verhaltenszustand nach AFTER_FORWARD; muss sich nach Observerstopp zum 1. Zielpunkt ausrichten
    -				LOG_DEBUG("P2 anfahren und dann zu P1, da P2 frei");
    -				break;
    -			}
    -		}
    +         uint8_t free2 = map_way_free(x_pos, y_pos, nextline.point2.x, nextline.point2.y, MAP_WAY_FREE_MARGIN);
    + 	     LOG_DEBUG("Wegfreiheit zu P2: %1d, Abstand P1: %1.0f P2: %1.0f, Abstand P1P2: %1.0f",free2,
    + 	       sqrt(get_dist(x_pos,y_pos,nextline.point1.x, nextline.point1.y)),
    + 	       sqrt(get_dist(x_pos,y_pos,nextline.point2.x, nextline.point2.y)),
    + 	       sqrt(get_dist(nextline.point1.x, nextline.point1.y,nextline.point2.x, nextline.point2.y)));
    +         if (free2 && (get_dist(nextline.point2.x,nextline.point2.y,x_pos,y_pos) >=300*300)) {
    +           set_nextline(nextline.point1, nextline.point2, True); //Werte egal da nextline nur getauscht
    +           track_state=AFTER_FORWARD;//muss sich hiernach zum Zielpunkt ausrichten
    +           next_behavstate=TURN_TO_NEAREST;  //Verhaltenszustand nach AFTER_FORWARD; muss sich nach Observerstopp zum 1. Zielpunkt ausrichten
    +           LOG_DEBUG("P2 anfahren und dann zu P1, da P2 frei");
    +           break;
    +         }
    +       }

     		// 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
    +/*#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, /*MAP_DRIVEN_THRESHOLD*/0); // Pfadplanung klappt nie ueber befahrenes Gebiet
    -			track_state = TURN_TO_DESTINATION;//muss sich hiernach zum Zielpunkt ausrichten
    +			bot_calc_wave(data, nextline.point1.x, nextline.point1.y, MAP_DRIVEN_THRESHOLD);// Pfadplanung
    +			track_state=TURN_TO_DESTINATION;//muss sich hiernach zum Zielpunkt ausrichten
     			break;
     		}
    -#else
    +#else*/
     		if (!free1) {
    -			uint8_t free2 = map_way_free(x_pos, y_pos, nextline.point2.x, nextline.point2.y, MAP_WAY_FREE_MARGIN);
    -			LOG_DEBUG("Wegfreiheit zu P2 ohne Pfadplanung: %1d",free2);
    +		 uint8_t free2 = map_way_free(x_pos, y_pos, nextline.point2.x, nextline.point2.y, MAP_WAY_FREE_MARGIN);
    +		 LOG_DEBUG("Wegfreiheit zu P2 ohne Pfadplanung: %1d, Mapwert: %1d",free2,map_get_field(nextline.point2));

    -			// falls Weg zu keinem Punkt frei, dann mit dem naechsten Stackweg tauschen; ist dieser auch nicht befahrbar,
    -			// wird dieser verworfen und der Weg
    -			if (!free2) {
    -				LOG_DEBUG("Stackweg nicht frei");
    +		// falls Weg zu keinem Punkt frei, dann mit dem naechsten Stackweg tauschen; ist dieser auch nicht befahrbar,
    +		// wird dieser verworfen und der Weg
    +		if (!free2) {
    +			LOG_DEBUG("Stackweg nicht frei");

    -				static trackpoint_t pt; //zum Merken beim Vertauschen
    +#ifdef GO_WITH_PATHPLANING
    +		//if (!free1) {
    +			bot_stop_observe(); // auf jeden Fall erst mal stoppen
    +			// Fuer Pfadplanung nur Strecken anfahren mit gewisser Laenge
    +			if (get_dist(nextline.point1.x, nextline.point1.y,nextline.point2.x, nextline.point2.y)>200*200) {
    +			 if (get_dist(nextline.point1.x,nextline.point1.y,x_pos,y_pos) >=300*300)	{
    +			  LOG_DEBUG("Weg nicht frei, >> Pfadplanung zu P1 << %1d %1d", nextline.point1.x, nextline.point1.y);
    +			 }
    +			 else {
    +				 set_nextline(nextline.point1, nextline.point2, True); //Werte egal da nextline nur getauscht
    +			     LOG_DEBUG("Weg nicht frei, >> Pfadplanung zu P2 nach Tausch wg. zu geringem Abstand << %1d %1d", nextline.point1.x, nextline.point1.y);
    +			 }
    +			 bot_calc_wave(data, nextline.point1.x, nextline.point1.y, MAP_DRIVEN_THRESHOLD);// Pfadplanung
    +			 track_state=TURN_TO_DESTINATION;//muss sich hiernach zum Zielpunkt ausrichten
    +			 break;
    +			}
    +			else {
    +			  LOG_DEBUG("Strecke zu kurz fuer Pfadplanung, evtl. Vertauschen");
    +			}
    +		//}
    +#else

    -				if (pop_stack_pos_line(&pt.point1, &pt.point2)) {
    +			static trackpoint_t pt; //zum Merken beim Vertauschen

    -					free1 = map_way_free(x_pos, y_pos, pt.point1.x, pt.point1.y, MAP_WAY_FREE_MARGIN);
    -					free2 = map_way_free(x_pos, y_pos, pt.point2.x, pt.point2.y, MAP_WAY_FREE_MARGIN);
    +			if (pop_stack_pos_line(&pt.point1, &pt.point2)) {

    -					if (!free1 && !free2) {
    -						LOG_DEBUG("Folgeweg auch nicht frei"); //Weg verworfen wenn nicht anfahrbar->naechsten Stackweg holen
    -						break;
    -					}
    +				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);

    -					//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.point1, nextline.point2); //wieder auf Stack
    -					push_stack_pos_line(pt.point1, pt.point2); //wieder auf Stack
    -					LOG_DEBUG("Weg mit Folgeweg getauscht");
    +				if (!free1 && !free2) {
    +					LOG_DEBUG("Folgeweg auch nicht frei"); //Weg verworfen wenn nicht anfahrbar->naechsten Stackweg holen
     					break;
     				}

    +				//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.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	// GO_WITH_PATHPLANING
    +		 }
     		}
    -#endif	// GO_WITH_PATHPLANING
    +//#endif	// GO_WITH_PATHPLANING
     		//naechster Verhaltenszustand zum Anfahren der naechsten Bahn mit Ausrichten auf den naeheren Punkt laut nextline
     		track_state = TURN_TO_NEAREST;
     		break;
    Index: C:/eclipse/workspace/ct-Bot/Changelog.txt
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/Changelog.txt	(revision 1585)
    +++ C:/eclipse/workspace/ct-Bot/Changelog.txt	(working copy)
    @@ -1,5 +1,9 @@
     CHANGELOG fuer c't-Bot
     ======================
    +2009-05-16 Frank Menzel [Menzelfr@xxxxxx]: diverse Aenderungen im drive_area-Verhalten
    +
    +2009-05-13 Frank Menzel [Menzelfr@xxxxxx]: Korrekturen im Pfadplanungsverhalten; Bug bei Endeerkennung beseitigt und Planung auf bereits befahrenem Gebiet ermoeglicht
    +
     2009-04-30 Frank Menzel [Menzelfr@xxxxxx]: Einzeichnen der Fahrspuren in drive_area zur Visualisierung der Arbeitsweise des Verhaltens sowie Korrekturen und mehr Debugausgaben

     2009-04-22 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Bugfix fuer map_2_sim_send()
    Index: C:/eclipse/workspace/ct-Bot/ct-Bot.h
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/ct-Bot.h	(revision 1585)
    +++ C:/eclipse/workspace/ct-Bot/ct-Bot.h	(working copy)
    @@ -59,7 +59,7 @@

     #define ADC_AVAILABLE		/*!< A/D-Konverter */

    -//#define MOUSE_AVAILABLE	/*!< Maus Sensor */
    +#define MOUSE_AVAILABLE	/*!< Maus Sensor */

     #define ENA_AVAILABLE		/*!< Enable-Leitungen */
     #define SHIFT_AVAILABLE		/*!< Shift Register */
    @@ -72,8 +72,8 @@

     #define POS_STORE_AVAILABLE		/*!< Positionsspeicher vorhanden */

    -//#define MAP_AVAILABLE			/*!< Aktiviert die Kartographie */
    -//#define MAP_2_SIM_AVAILABLE	/*!< Sendet die Map zur Anzeige an den Sim */
    +#define MAP_AVAILABLE			/*!< Aktiviert die Kartographie */
    +#define MAP_2_SIM_AVAILABLE	/*!< Sendet die Map zur Anzeige an den Sim */

     //#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 1585)
    +++ C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h	(working copy)
    @@ -34,7 +34,7 @@

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

    -//#define BEHAVIOUR_PATHPLANING_AVAILABLE /*!< Pfadplanungsverhalten  */
    +#define BEHAVIOUR_PATHPLANING_AVAILABLE /*!< Pfadplanungsverhalten  */
     //#define BEHAVIOUR_DRIVE_STACK_AVAILABLE /*!< Abfahren der auf dem Stack gesicherten Koordinaten */

     //#define BEHAVIOUR_OLYMPIC_AVAILABLE	/*!< Olympiadenverhalten vorhanden? */
    @@ -58,7 +58,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 */

     //#define BEHAVIOUR_LINE_SHORTEST_WAY_AVAILABLE /*!< Linienfolger ueber Kreuzungen zum Ziel */

    Index: C:/eclipse/workspace/ct-Bot/.cdtbuild
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/.cdtbuild	(revision 1585)
    +++ C:/eclipse/workspace/ct-Bot/.cdtbuild	(working copy)
    @@ -10,8 +10,8 @@
     <option id="gnu.c.compiler.option.include.paths.387475860" superClass="gnu.c.compiler.option.include.paths" valueType="includePath">
     <listOptionValue builtIn="false" value="&quot;${ProjDirPath}&quot;"/>
     <listOptionValue builtIn="false" value="&quot;${ProjDirPath}/include&quot;"/>
    -<listOptionValue builtIn="false" value="&quot;C:\Programme\MinGW\include&quot;"/>
    -<listOptionValue builtIn="false" value="&quot;C:\Programme\pthreads\pthreads.2&quot;"/>
    +<listOptionValue builtIn="false" value="&quot;C:\MinGW\include&quot;"/>
    +<listOptionValue builtIn="false" value="&quot;C:\pthreads\pthreads.2&quot;"/>
     </option>
     <option id="gnu.c.compiler.option.preprocessor.def.symbols.1823712582" superClass="gnu.c.compiler.option.preprocessor.def.symbols" valueType="definedSymbols">
     <listOptionValue builtIn="false" value="PC"/>
    @@ -27,8 +27,8 @@
     <listOptionValue builtIn="false" value="pthreadGC2"/>
     </option>
     <option id="gnu.c.link.option.paths.142594843" superClass="gnu.c.link.option.paths" valueType="stringList">
    -<listOptionValue builtIn="false" value="&quot;C:\Programme\pthreads\Pre-built.2\lib&quot;"/>
    -<listOptionValue builtIn="false" value="&quot;C:\Programme\MinGW\lib&quot;"/>
    +<listOptionValue builtIn="false" value="&quot;C:\pthreads\Pre-built.2\lib&quot;"/>
    +<listOptionValue builtIn="false" value="&quot;C:\MinGW\lib&quot;"/>
     </option>
     <option id="gnu.c.link.option.noshared.1812776571" superClass="gnu.c.link.option.noshared" value="true" valueType="boolean"/>
     </tool>
    Index: C:/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 @@
    +#Tue May 12 19:17:54 CEST 2009
    +eclipse.preferences.version=1
    +indexerId=org.eclipse.cdt.core.fastIndexer
    Index: C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs
    ===================================================================
    --- C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs	(revision 1585)
    +++ C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs	(working copy)
    @@ -1,13 +1,15 @@
    -#Thu Mar 29 15:23:55 CEST 2007
    -cdt.managedbuild.config.gnu.exe.debug.1197043799/internalBuilder/enabled=false
    -cdt.managedbuild.config.gnu.exe.debug.1197043799/internalBuilder/ignoreErr=true
    -eclipse.preferences.version=1
    -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n
    -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n
    -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1197043799.2016949464=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n
    -environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n
    -environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n
    -environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1197043799.2016949464=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n
    -environment/project=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment/>\n
    -environment/project/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment/>\n
    -environment/project/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable delimiter\="\:" name\="PATH" operation\="append" value\="/usr/local/avr/bin"/>\n</environment>\n
    +#Tue May 12 19:19:04 CEST 2009
    +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

- Antworten:
  - Re: [ct-bot] Aenderungen im Pfadplanungsverhalten->area-Verhalten
    - From: Timo Sandmann

- Referenzen:
  - AW: [ct-bot] Aenderungen im Pfadplanungsverhalten->area-Verhalten
    - From: Frank Menzel

- Voriger (Datum): AW: [ct-bot] Aenderungen im Pfadplanungsverhalten->area-Verhalten, Frank Menzel
- Nächster (Datum): Re: [ct-bot] Aenderungen im Pfadplanungsverhalten->area-Verhalten, Timo Sandmann
- Voriger (Thread): AW: [ct-bot] Aenderungen im Pfadplanungsverhalten->area-Verhalten, Frank Menzel
- Nächster (Thread): Re: [ct-bot] Aenderungen im Pfadplanungsverhalten->area-Verhalten, Timo Sandmann
- Indizes:
  - Datum
  - Thread

- Aktuelles Heft
- Aktuelles Editorial
- Websites aktuell

#### Support

- Hotline & FAQ
- Treiber & BIOS
- Firmenkontakte

#### Download

- Software zu c't
- Software-Verzeichnis
- c't-Projekte
- Testbilder & Vorlagen

#### Magazin

- Heftarchiv
- c't specials
- Leserforum
- c't-Bildmotive
- Schlagseite

#### Info

- Abo & Heft
- Veranstaltungen
- Mediadaten
- Kontakt
- Impressum

Kleinanzeigen und
Veranstaltungen

- Datenschutzhinweis
- Impressum
- Kritik, Anregungen bitte an c't-WWW
- Mediadaten
- Copyright © 2009 Heise Zeitschriften Verlag
- International: The H, The H Security, The H Open Source, heise online Polska, heise Security Polska, heise Open Source Polska, heise Networks Polska