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