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