c't

c't-Projekte - Mailinglisten


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

[ct-bot] Erweiterung Pfadplanung

Absender: Frank Menzel
Datum: Fr, 27.03.2009 13:25:01


Hallo,
habe das Pfadplanungsverhalten erweitert um die Möglichkeit, nur auf
bereits befahrenem Gebiet zu planen. Ebenfalls habe ich dieses erweitert
um die 8er-Nachbarschaft, d.h. der bot kann nun auch schräge Wege
fahren. Ist MAP_2_SIM_AVAILABLE eingeschaltet, wird nun ebenfalls
mittels roter Linien der berechnete Pfadweg in der Map visualisiert.
 
Gruß, Frank
Index: C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_pathplaning.c
===================================================================
--- C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_pathplaning.c	(revision 1568)
+++ C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_pathplaning.c	(working copy)
@@ -60,8 +60,7 @@
 #define LOG_DEBUG(a, ...) {}
 #endif
 
-// Einzeichenn des geplanten Weges in die Highres-Map als helle Punkte
-//#define SHOW_PATH_IN_MAP
+// Rotes Einzeichenn des geplanten Weges in die Highres-Map ueber MAP_2_SIM_AVAILABLE
 
 /****** fuer geringaufloesende Map zur Pfadplanung  --ACHTUNG ONLINE_SHRINK einschalten -- *****************/
 
@@ -123,6 +122,8 @@
 
 static uint8_t wave_state = 0; /*!< Statusvariable */
 
+static int8_t map_compare_haz = 0; /*!< Vergleichswert unterhalb dem Hindernioswert gesetzt wird */
+
 /*!
  * Konvertiert eine Lowres-Kartenkoordinate in eine Weltkoordinate
  * @param map_koord	Kartenkoordinate
@@ -211,7 +212,7 @@
 	uint8_t i, j;
 	LOG_DEBUG("Delete von 0-%1d", MAP_SECTION_POINTS_LOWRES);
 	for (j = 0; j < MAP_SECTIONS_LOWRES*MAP_SECTION_POINTS_LOWRES; j++) {
-		LOG_DEBUG("Zeile %1d", j);
+		//LOG_DEBUG("Zeile %1d", j);
 		for (i = 0; i < MAP_SECTIONS_LOWRES*MAP_SECTION_POINTS_LOWRES; i++) {
 			access_field_lowres((position_t) {i, j}, 0, 1);
 					//LOG_DEBUG("Spalte %1d",i);
@@ -247,9 +248,9 @@
 //TODO: Ganze Zelle sollte als Hindernis eingetragen werden, sobald auch nur ein Feld der Map als belegt markiert ist!
 
 			mapavg = map_get_average(map_to_world_lowres(x), ym, average_val); // probiert: 80 zu viele Hind 60 40 gut, 30 gut, 20 gut weniger auch weniger Hindernisse
-			if (mapavg < 0) { // hier koennte man bei Weg nicht findbar auch Schwellenwert schrittweise veraendern
+			if (mapavg < map_compare_haz)  // Vergleichswert; damit kann auch auf nur befahrenen Wegen geplant werden bei Wert 30
 				access_field_lowres((position_t) {x, y}, 1, 1);
-			}
+			
 		}
 	}
 
@@ -366,10 +367,10 @@
 				// Kennung fuer gueltigen Nachbarn wieder ruecksetzen
 				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
+				// abarbeiten aller 8 Nachbarn zu einem Punkt der Queue, auch wenn schon gefunden wurde zwecks guter Pfadfindung; ein True wird nicht ueberschrieben
 				for (j=-1; j<=1; j++) {
 					for (i=-1; i<=1; i++) {
-						if ((j == 0 && i != 0) || (i == 0 && j != 0)) {
+						if (!(i == 0 && j == 0)) {
 							position_t tmp = pos;
 							tmp.x += i;
 							tmp.y += j;
@@ -412,6 +413,13 @@
 		endreached = False; // Schleifenabbruchvar init.
 		wavecounter = 0; // Wellenzaehler init.
 
+#ifdef   MAP_2_SIM_AVAILABLE
+        position_t lastpos ;
+        position_t aktpos ;
+        lastpos.x=0;  //initialisieren
+        lastpos.y=0;
+#endif
+
 		// Map durchlaufen und den Nachbarn mit immer niedrigerem Mapwert verfolgen bis Wellenstartpunkt erreicht wurde
 		while (!endreached) {
 			neighbour_found = False;
@@ -420,14 +428,14 @@
 			for (j=-1; (j<=1) && !endreached; j++)
 			for (i=-1; (i<=1) && !endreached; i++)
 
-			// Vergleich nur fuer die richtigen 4 Nachbarn und wenn Wellenstartpunkt noch nicht erreicht
-			if (((j == 0 && i != 0) || (i == 0 && j != 0)) && !(pos.x + i == endkoord.x && pos.y + j == endkoord.y)) {
+			// Vergleich fuer die 8 Nachbarn und wenn Wellenstartpunkt noch nicht erreicht
+			if ((!(i == 0 && j == 0)) && !(pos.x + i == endkoord.x && pos.y + j == endkoord.y)) {
 				// nur gueltige Koords gecheckt
+				
 				position_t tmp = pos;
 				tmp.x += i;
 				tmp.y += j;
 				minval = access_field_lowres(tmp, 0, 0); // Mapwert auslesen
-
 				// 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;
@@ -460,8 +468,13 @@
 
 				// Der zu fahrende Pfad kann in der Highres-Map als helle Punkte eingezeichnet werden; dazu nach Pfadplanung und zu Beginn des Fahrverhaltens
 				// abbrechen und der zu fahrende Weg ist nun in der Highres-map eingezeichnet; die Routine access_field() muss dazu aber auch in der map.h deklariert werden
-#ifdef   SHOW_PATH_IN_MAP
-				access_field(world_to_map(map_to_world_lowres(nextdest.x)), world_to_map(map_to_world_lowres(nextdest.y)), 127, 1);
+#ifdef   MAP_2_SIM_AVAILABLE
+				//access_field(world_to_map(map_to_world_lowres(nextdest.x)), world_to_map(map_to_world_lowres(nextdest.y)), 127, 1);
+				aktpos.x=world_to_map(map_to_world_lowres(nextdest.x));
+				aktpos.y=world_to_map(map_to_world_lowres(nextdest.y));
+    			if (lastpos.x!=0 && lastpos.y!=0)
+    			  map_draw_line(lastpos,aktpos,1);  //Pfadpunkte Rot darstellen
+				lastpos=aktpos;				
 #endif
 			}
 			wavecounter++; // Wellencounter erhoehen; dient hier nur fuer Abbruchbedingung nach erreichen eines bestimmten Zaehlerstandes
@@ -525,11 +538,13 @@
 /*!
  * Rufe das Wave-Verhalten auf
  * @param *caller	Der obligatorische Verhaltensdatensatz des Aufrufers
- */
-void bot_do_calc_wave(Behaviour_t * caller) {
+ * @param map_compare Map-Vergleichswert; Mapwerte kleiner dem Wert werden als Hindernisse eingetragen
+ */ 
+void bot_do_calc_wave(Behaviour_t * caller, int8_t map_compare) {
 	switch_to_behaviour(caller, bot_calc_wave_behaviour, OVERRIDE);
 	wave_state = 0;
-	average_val = 40; // beginnend mit Radius 40 mm zum Eintragen der Hindernisse aus der Highres- in die Planungs-Lowres-Karte
+	average_val = 30; // beginnend mit Radius 30 mm zum Eintragen der Hindernisse aus der Highres- in die Planungs-Lowres-Karte
+    map_compare_haz = map_compare; //Wert setzen, unterhalb dessen Hindernis gesetzt wird
 
 	LOG_DEBUG("Start Welle vom Zielpunkt %1d %1d", startwave.x, startwave.y);
 
@@ -549,13 +564,13 @@
  * @param *caller	Der obligatorische Verhaltensdatensatz des Aufrufers
  * @param dest_x    X-World-Zielkoordinate
  * @param dest_y    Y-World-Zielkoordinate
+ * @param map_compare Map-Vergleichswert; Mapwerte kleiner dem Wert werden als Hindernisse eingetragen
  */
-void bot_calc_wave(Behaviour_t * caller, int16_t dest_x, int16_t dest_y) {
+void bot_calc_wave(Behaviour_t * caller, int16_t dest_x, int16_t dest_y, int8_t map_compare) {
 	// Zielpunkt setzen, ab dem die Welle losgeht
 	bot_set_destination(world_to_map_lowres(dest_x),
 			world_to_map_lowres(dest_y)); // Weltkoords in Lowres-Mapkoords umrechnen
-
-	bot_do_calc_wave(caller);
+ 	bot_do_calc_wave(caller, map_compare);
 }
 
 #ifdef DEBUG_PATHPLANING
@@ -600,12 +615,13 @@
 
 	case RC5_CODE_4:
 		RC5_Code = 0;
-		delete_lowres();
+		//delete_lowres();
+		bot_do_calc_wave(0,30);  //nur auf bereits befahrenem Gebiet Weg zum Ziel planen
 		break;
 
 	case RC5_CODE_5:
 		RC5_Code = 0;
-		bot_do_calc_wave(0);
+		bot_do_calc_wave(0,0);  // Aufruf mit Map-Vergleichswert 0; Werte darunter gelten als Hindernisse
 		break;
 
 #ifdef DEBUG_PATHPLANING
@@ -630,7 +646,7 @@
 	display_cursor(1, 1);
 	display_printf("-Pathplaning-");
 	display_cursor(2, 1);
-	display_printf("4:Delete");
+	display_printf("4:PlanOnDrivenArea");//nur auf befahrenem Gebiet planen
 	display_cursor(3, 1);
 	display_printf("5:GoPlaning");
 	display_cursor(4, 1);
Index: C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c
===================================================================
--- C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c	(revision 1568)
+++ C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c	(working copy)
@@ -924,7 +924,7 @@
 		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);
+			bot_calc_wave(data, nextline.point1.x, nextline.point1.y, MAP_TRACKVAL);
 		}
 #else
 		uint8_t free2 = map_way_free(x_pos, y_pos, nextline.point2.x, nextline.point2.y);
Index: C:/eclipse/workspace/ct-Bot/Changelog.txt
===================================================================
--- C:/eclipse/workspace/ct-Bot/Changelog.txt	(revision 1568)
+++ C:/eclipse/workspace/ct-Bot/Changelog.txt	(working copy)
@@ -1,5 +1,7 @@
 CHANGELOG fuer c't-Bot
 ======================
+2009-03-27 Frank Menzel [Menzelfr@xxxxxx]: Pfadplanungsverhalten erweitert: schraeges Fahren und Planung nur auf befahrenem Gebiet ermoeglicht sowie Einzeichnen des Pfadweges (Rot) via MAP_2_SIM_AVAILABLE
+
 2009-03-18 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Delay-Code an aktuellen Timer-Code angepasst
 
 2009-03-16 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: ToDo-Punkte erledigt: bot-2-pc und bot-2-sim vereinheitlicht zu bot-2-sim;
Index: C:/eclipse/workspace/ct-Bot/ct-Bot.h
===================================================================
--- C:/eclipse/workspace/ct-Bot/ct-Bot.h	(revision 1568)
+++ 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,7 +72,7 @@
 
 #define POS_STORE_AVAILABLE		/*!< Positionsspeicher vorhanden */
 
-//#define MAP_AVAILABLE			/*!< Aktiviert die Kartographie */
+#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 */
Index: C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h
===================================================================
--- C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h	(revision 1568)
+++ C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h	(working copy)
@@ -34,8 +34,8 @@
 
 #define BEHAVIOUR_SERVO_AVAILABLE 	/*!< Kontrollverhalten fuer die Servos */
 
-//#define BEHAVIOUR_PATHPLANING_AVAILABLE /*!< Pfadplanungsverhalten  */
-//#define BEHAVIOUR_DRIVE_STACK_AVAILABLE /*!< Abfahren der auf dem Stack gesicherten Koordinaten */
+#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/include/bot-logic/behaviour_pathplaning.h
===================================================================
--- C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_pathplaning.h	(revision 1568)
+++ C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_pathplaning.h	(working copy)
@@ -55,14 +55,16 @@
  * @param *caller	Der obligatorische Verhaltensdatensatz des Aufrufers
  * @param dest_x    X-World-Zielkoordinate
  * @param dest_y    Y-World-Zielkoordinate
+ * @param map_compare Map-Vergleichswert; Mapwerte kleiner dem Wert werden als Hindernisse eingetragen 
  */
-void bot_calc_wave(Behaviour_t * caller, int16_t dest_x, int16_t dest_y);
+void bot_calc_wave(Behaviour_t * caller, int16_t dest_x, int16_t dest_y, int8_t map_compare);
 
 /*!
  * Rufe das Wave-Verhalten auf
  * @param *caller	Der obligatorische Verhaltensdatensatz des Aufrufers
+ * @param map_compare Map-Vergleichswert; Mapwerte kleiner dem Wert werden als Hindernisse eingetragen 
  */
-void bot_do_calc_wave(Behaviour_t * caller);
+void bot_do_calc_wave(Behaviour_t * caller, int8_t map_compare);
 
 /*!
  * Wave-Verhalten; berechnet die Welle ausgehend vom Zielpunkt bis zur Botposition; dann wird diese zurueckverfolgt und sich der Pfad
Index: C:/eclipse/workspace/ct-Bot/include/map.h
===================================================================
--- C:/eclipse/workspace/ct-Bot/include/map.h	(revision 1568)
+++ C:/eclipse/workspace/ct-Bot/include/map.h	(working copy)
@@ -193,6 +193,14 @@
  * @param color	Farbe der Linien: 0=gruen, 1=rot, sonst schwarz
  */
 void map_draw_rect(position_t from, position_t to, uint8_t width, uint8_t color);
+
+/*!
+ * Konvertiert eine Weltkoordinate in eine Kartenkoordinate
+ * @param koord	Weltkoordiante
+ * @return		Kartenkoordinate
+ */
+uint16_t world_to_map(int16_t koord);  //zum Visualisieren pathplaning notwendig
+
 #endif	// MAP_2_SIM_AVAILABLE
 
 #ifdef PC
Index: C:/eclipse/workspace/ct-Bot/map.c
===================================================================
--- C:/eclipse/workspace/ct-Bot/map.c	(revision 1568)
+++ C:/eclipse/workspace/ct-Bot/map.c	(working copy)
@@ -291,7 +291,7 @@
  * @param koord	Weltkoordiante
  * @return		Kartenkoordinate
  */
-static uint16_t world_to_map(int16_t koord) {
+uint16_t world_to_map(int16_t koord) {
 #if (1000 / MAP_RESOLUTION) * MAP_RESOLUTION != 1000
 	#warning "MAP_RESOLUTION ist kein Teiler von 1000, Code in world_to_map() anpassen!"
 #endif
@@ -449,7 +449,7 @@
  * @param value	Neuer Wert des Feldes (> 0 heisst frei, <0 heisst belegt)
  * @param set	0 zum Lesen, 1 zum Schreiben
  */
-static int8_t access_field(uint16_t x, uint16_t y, int8_t value, uint8_t set) {
+int8_t access_field(uint16_t x, uint16_t y, int8_t value, uint8_t set) {
 	uint16_t index_x, index_y;
 
 	// Suche die Section heraus
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 @@
+#Wed Mar 25 15:15:20 CET 2009
+eclipse.preferences.version=1
+indexerId=org.eclipse.cdt.core.fastIndexer