c't Projekte - c't-Bot und c't-Sim - Mailinglisten
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=""${ProjDirPath}""/> <listOptionValue builtIn="false" value=""${ProjDirPath}/include""/> -<listOptionValue builtIn="false" value=""C:\Programme\MinGW\include""/> -<listOptionValue builtIn="false" value=""C:\Programme\pthreads\pthreads.2""/> +<listOptionValue builtIn="false" value=""C:\MinGW\include""/> +<listOptionValue builtIn="false" value=""C:\pthreads\pthreads.2""/> </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=""C:\Programme\pthreads\Pre-built.2\lib""/> -<listOptionValue builtIn="false" value=""C:\Programme\MinGW\lib""/> +<listOptionValue builtIn="false" value=""C:\pthreads\Pre-built.2\lib""/> +<listOptionValue builtIn="false" value=""C:\MinGW\lib""/> </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