Absender: Frank Menzel
Datum: Mo, 27.10.2008 16:42:28
Hallo, habe jetzt die Pfadplanung in das area-Verhalten eingebaut. Das Area-Verhalten verwendet jetzt ein eigenes Array zur Speicherung der Wegalternativen, siehe Mailingliste. Gruß, Frank
Index: C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c =================================================================== --- C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c (revision 1498) +++ C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c (working copy) @@ -42,12 +42,27 @@ #include "map.h" #include "timer.h" #include "math_utils.h" -#include "pos_stack.h" #include "log.h" -//#define DEBUG_DRIVE_AREA // Schalter fuer Debug-Code +//#define DEBUG_MEASURE_TIME // Schalter fuer Debug-Code Zeitmessung Map-Zugriffszeit +//#define DEBUG_BEHAVIOUR_AREA // Schalter fuer Debugausgaben + +#ifndef LOG_AVAILABLE + #undef DEBUG_BEHAVIOUR_AREA +#endif +#ifndef DEBUG_BEHAVIOUR_AREA + #undef LOG_DEBUG + #define LOG_DEBUG(a, ...) {} +#endif + + +#ifdef BEHAVIOUR_PATHPLANING_AVAILABLE + #define GO_WITH_PATHPLANING // auskommentieren falls ohne Pfadplanung +#endif + + /*! nur alle X-Millisekunden Mapzugriff der Observer-Verhalten */ #define CORRECTION_DELAY 700 @@ -93,10 +108,8 @@ /*! Strukturtyp fuer die beobachteten Nachbar-Bahnen; fuer Links und Rechts bestehend aus 2 Punkten */ typedef struct { - int16_t point1x; /*!< Einzelne Punkte */ - int16_t point1y; - int16_t point2x; - int16_t point2y; + position_t point1; + position_t point2; } trackpoint_t; /*! Nachbarbahnen links und rechts */ @@ -109,7 +122,127 @@ /*! gemerkte letzte Position des Bots, um nur aller xx gefahrender mm auf Map zuzugreifen */ static trackpoint_t observe_lastpos; + /* ================================================================================== + * ===== Definition eines eigenen Linienspeichers zum Merken der Alternativstrecken = + * ==================================================================================*/ + +/*! Implementierung eines eigenen Linienspeichers; Stack nicht verwendbar wegen dem sonst identischen Speicherbereich fuer Pfadplanung und drive_Stack-Verhalten */ + +/*! + * Linie wird definiert als Anfangs- und Endpunkt + */ +typedef struct { + position_t p1; /*!< Anfangspunkt der Linie */ + position_t p2; /*!< Endpunkt der Linie */ +} line_t; + +#define LINE_STORE_SIZE 32 /*!< Speichergroesse, muss durch 2 teilbar sein */ +static line_t line_stack[LINE_STORE_SIZE]; /*!< Datenspeicher */ +static uint8_t linecounter = 0; /*!< Anzahl der gespeicherten Linien, bestehend jeweils aus Start- und Endpunkt */ +//uint8_t line_store_sp = 0; /*!< Zeiger auf Arrayindex */ + + +/*! + * Leert den Linienspeicher + */ +void line_store_clear(void) { + linecounter = 0; +} + +/*! + * Speicher voll? + * @return True falls Speicher voll, sonst False + */ +static uint8_t linestore_is_full(void) { + return linecounter == LINE_STORE_SIZE; +} + +/*! + * Speicher leer? + * @return True falls Speicher leer sonst False + */ +static uint8_t linestore_is_empty(void) { + return linecounter == 0; +} + +/*! + * Speichern einer Linienkoordinate auf den Stack + * @param pos X/Y-Koordinaten des zu sichernden Punktes + * @param firstpoint True falls der 1. der beiden Linienpunkte uebergeben wurde, False fuer den anderen Linienpunkt + * @return True wenn erfolgreich sonst False wenn Array voll ist + */ +uint8_t line_store_push_point(position_t pos, uint8_t firstpoint) { + if (linestore_is_full()) { + return False; + } + if (firstpoint) // den richtigen Punkt in die entsprechende Koordinate speichern + line_stack[linecounter].p1 = pos; + else + line_stack[linecounter].p2 = pos; + + linecounter++; + return True; +} + +/*! + * Speichern einer Linie auf den Stack durch Ubergabe des Start- und Endpunktes + * @param p1 zu sichernder Startpunktes + * @param p2 zu sichernder Endpunkt + * @return True wenn erfolgreich sonst False wenn Linienstack voll ist + */ +uint8_t line_store_push(position_t p1, position_t p2) { + if (!line_store_push_point(p1,True)) // 1. Linienpunkt in Stack + return False; + if (!line_store_push_point(p2,False)) // 2. Linienpunkt in Stack + return False; + + LOG_DEBUG("Linie gepusht: P1: %1d %1d P2: %1d %1d, linecounter %1d",p1.x,p1.y,p2.x,p2.y,linecounter); + return True; + + + +} + +/*! + * Pop-Routine zur Rueckgabe des letzten auf dem Linienstack gepushten Punktes + * @param *pos Zeiger auf Rueckgabewert der Linienkoordinate + * @param firstpoint True fuer den 1. Linienpunkt, Flase fuer den 2. Linienpunkt + * @return False falls Pop nicht erfolgreich, d.h. kein Punkt mehr auf dem Stack, sonst True nach erfolgreichem Pop + */ +uint8_t line_store_pop_point(position_t * pos, uint8_t firstpoint) { + if (linestore_is_empty()) { + return False; + } + linecounter--; + + *pos=(firstpoint)?line_stack[linecounter].p1:line_stack[linecounter].p2; + + return True; +} + +/*! + * Pop-Routine zur Rueckgabe der letzten auf dem Stack gespeicherten Linie + * @param *pos1 Zeiger auf Rueckgabe-Speicher der 1. Linienkoordinate + * @param *pos2 Zeiger auf Rueckgabe-Speicher der 2. Linienkoordinate + * @return False falls Pop nicht erfolgreich, d.h. kein Punkt mehr auf dem Stack, sonst True nach erfolgreichem Pop + */ +uint8_t line_store_pop(position_t * pos1, position_t * pos2) { + if (!line_store_pop_point(pos2,False)) + return False; + if (!line_store_pop_point(pos1,True)) + return False; + + LOG_DEBUG("Popline P1: %1d %1d P2: %1d %1d",pos1->x,pos1->y,pos2->x,pos2->y); + return True; +} + +/* ================================================================================== + * ===== Ende des Linienstacks ====================================================== + * ==================================================================================*/ + + +/* ================================================================================== * ===== Start der allgemein fuer diese Verhalten notwendigen Routinen ============== * ==================================================================================*/ @@ -123,89 +256,58 @@ /*! * liefert den Wert eines Feldes als Durchschnittwert laut Map in bestimmtem Umkreis - * @param x X-Weltkoordinate - * @param y Y-Weltkoordinate + * @param point XY-Weltkoordinate * @return Durchschnittswert des Feldes (>0 heisst frei, <0 heisst belegt) */ -static int8_t map_get_field(int16_t x, int16_t y) { +static int8_t map_get_field(position_t point) { #ifdef DEBUG_DRIVE_AREA uint8_t result; - TIMER_MEASURE_TIME(result = map_get_average(x, y, 50)); + TIMER_MEASURE_TIME(result = map_get_average(point.x, point.y, 50)); return result; #else - return map_get_average(x, y, 50); // Mapwert Durchschnitt + return map_get_average(point.x, point.y, 50); // Mapwert Durchschnitt #endif } /*! * Speichert eine Line auf dem Stack, d.h. den Start- und Endpunkt der Linie - * @param x1 X-Koordinate des ersten Punktes der Linie - * @param y1 Y-Koordinate des ersten Punktes der Linie - * @param x2 X-Koordinate des zweiten Punktes der Linie - * @param y2 Y-Koordinate des zweiten Punktes der Linie + * @param point1 XY-Koordinate des ersten Punktes der Linie + * @param point2 XY-Koordinate des zweiten Punktes der Linie */ -static void push_stack_pos_line(int16_t x1, int16_t y1, int16_t x2, int16_t y2) { +static void push_stack_pos_line(position_t point1, position_t point2) { - if ((x1 == 0 && y1 == 0) || (x2 == 0 && y2 == 0)) + if ((point1.x == 0 && point1.y == 0) || (point2.x == 0 && point2.y == 0)) return; // ungueltige Werte werden nicht uebernommen - // Push der Einzelpunkte - pos_store_push(x1, y1); - pos_store_push(x2, y2); + // Push der Linienpunkte + line_store_push(point1, point2); } -/*! - * Holt eine Line vom Stack, d.h. die beiden Punkte der Linie werden zurueckgegeben - * @param *x1 X-Koordinate des ersten Punktes der Linie - * @param *y1 Y-Koordinate des ersten Punktes der Linie - * @param *x2 X-Koordinate des zweiten Punktes der Linie - * @param *y2 Y-Koordinate des zweiten Punktes der Linie - * @return True wenn erfolgreich, False falls Stack leer ist - */ -static uint8_t pop_stack_pos_line(int16_t * x1, int16_t * y1, int16_t * x2, - int16_t * y2) { - int16_t x_i; - int16_t y_i; - if (!pos_store_pop(&x_i, &y_i)) - return False; - *x1 = x_i; - *y1 = y_i; - if (!pos_store_pop(&x_i, &y_i)) - return False; - *x2 = x_i; - *y2 = y_i; - return True; -} - /*! * Hilfsroutine, um 2 Punkte in die Merkvariable zu speichern - * @param *lastpointx X-Merkvariable - * @param *lastpointy Y-Merkvariable + * @param *lastpoint XY-Merkvariable * @param mapx zu merkender X-Wert * @param mapy zu merkender Y-Wert */ -static void set_point_to_lastpoint(int16_t * lastpointx, int16_t * lastpointy, - int16_t mapx, int16_t mapy) { - *lastpointx = mapx; - *lastpointy = mapy; +static void set_point_to_lastpoint(position_t * lastpoint, int16_t mapx, int16_t mapy) { + lastpoint->x = mapx; + lastpoint->y = mapy; } /*! * Liefert True, wenn der Abstand zwischen den beiden Punkten gueltig ist, d.h. erreicht oder ueberschritten wurde - * @param xs World-Koordinate des zu untersuchenden Punktes - * @param ys World-Koordinate des zu untersuchenden Punktes - * @param xd World-Koordinate des Zielpunktes - * @param yd World-Koordinate des Zielpunktes + * @param point_s World-XY-Koordinate des zu untersuchenden Punktes + * @param point_d World-XY-Koordinate des Zielpunktes * @return True bei Erreichen des Abstandes zwischen den beiden Punkten sonst False */ -static uint8_t dist_valid(int16_t xs, int16_t ys, int16_t xd, int16_t yd) { +static uint8_t dist_valid(position_t point_s, position_t point_d) { // falls ein Punktepaar noch 0 hat, so ist dies ungueltig - if ((xs == 0 && ys == 0) || (xd == 0 && yd == 0)) + if ((point_s.x == 0 && point_s.y == 0) || (point_d.x == 0 && point_d.y == 0)) return False; /* Abstandsermittlung nach dem guten alten Pythagoras ohne Ziehen der Wurzel */ - return (get_dist(xs, ys, xd, yd) > DIST_VALID_QUAD); + return (get_dist(point_s.x, point_s.y, point_d.x, point_d.y) > DIST_VALID_QUAD); } /*! @@ -216,20 +318,19 @@ * @param *point berechnete Map-Weltkoordinate * @return Mapwert an dem ermittelten Punkt in Blickrichtung zum Abstand dist */ -static int8_t getpoint_side_dist(uint16_t sidedist, int16_t dist, int8_t side, - position_t * point) { +static int8_t getpoint_side_dist(uint16_t sidedist, int16_t dist, int8_t side, position_t * point) { // Berechnung je nach zu blickender Seite des Bots *point = (side == TRACKLEFT) ? calc_point_in_distance(heading, DISTSENSOR_POS_FW + dist, sidedist) : calc_point_in_distance( heading, DISTSENSOR_POS_FW + dist, -sidedist); // Rueckgabe des Mapwertes -#ifdef DEBUG_DRIVE_AREA +#ifdef DEBUG_MEASURE_TIME uint8_t result; - TIMER_MEASURE_TIME(result = map_get_field(point->x, point->y)); + TIMER_MEASURE_TIME(result = map_get_field(*point)); return result; #else - return map_get_field(point->x, point->y); + return map_get_field(*point); #endif } @@ -241,19 +342,18 @@ /*! Prueft ob der Bot schon eine bestimmte Strecke gefahren ist seit dem letzten Observerdurchgang - * @param *last_xpoint letzte gemerkte X-Koordinate - * @param *last_ypoint letzte gemerkte Y-Koordinate + * @param *last letzte gemerkte XY-Koordinate * @return True, wenn Bot schon gewisse Strecke gefahren ist und Map zu checken ist sonst False */ -static uint8_t check_map_after_distance(int16_t * last_xpoint, int16_t * last_ypoint) { +static uint8_t check_map_after_distance(position_t * last) { // Abstand seit letztem Observerlauf ermitteln - uint16_t diff = get_dist(x_pos, y_pos, *last_xpoint, *last_ypoint); + uint16_t diff = get_dist(x_pos, y_pos, last->x, last->y); //erst nach gewissem Abstand oder gleich bei noch initialem Wert Mappruefung - if ((diff >= MAPACCESS_AFTER_DISTANCE_QUAD) || (*last_xpoint==0 && *last_ypoint==0)) { - *last_xpoint = x_pos; - *last_ypoint = y_pos; + if ((diff >= MAPACCESS_AFTER_DISTANCE_QUAD) || (&last->x==0 && &last->y==0)) { + last->x = x_pos; + last->y = y_pos; return True; } return False; @@ -281,8 +381,8 @@ } // nur wenn bot seit letztem Zugriff gewisse Strecke gefahren ist erfolgt Mapzugriff; Check erfolgt seitenabhaengig - uint8_t mapaccess = (checkside == TRACKLEFT) ? check_map_after_distance(&observe_lastpos.point1x, &observe_lastpos.point1y) : - check_map_after_distance(&observe_lastpos.point2x,&observe_lastpos.point2y); + uint8_t mapaccess = (checkside == TRACKLEFT) ? check_map_after_distance(&observe_lastpos.point1) : + check_map_after_distance(&observe_lastpos.point2); // Schluss bei Endeanforderung oder noch nicht weit genug gefahren if (endrequest || !mapaccess) { @@ -300,8 +400,8 @@ // hier ist also der Trackpunkt frei, dann merken und zur Endpunktsuche im Observer // zuweisen der Initwerte in die richtigen Puntvars - set_point_to_lastpoint(&observe->point1x, &observe->point1y, map_pos.x, map_pos.y); - set_point_to_lastpoint(&observe->point2x, &observe->point2y, 0, 0); + set_point_to_lastpoint(&observe->point1, map_pos.x, map_pos.y); + set_point_to_lastpoint(&observe->point2, 0, 0); // Startpunkt gefunden return True; @@ -324,7 +424,7 @@ * @return True falls Endpunkt auf der Seite ermittelt werden konnte sonst False */ static uint8_t observe_get_endpoint(int8_t checkside, uint8_t * behavstate, - trackpoint_t * observer, int16_t * lastpointx, int16_t * lastpointy) { + trackpoint_t * observer, position_t * lastpoint) { position_t map_pos; int8_t mapval = 0; @@ -333,8 +433,8 @@ // nur wenn bot seit letztem Zugriff gewisse Strecke gefahren ist erfolgt Mapzugriff - uint8_t mapaccess = (checkside == TRACKLEFT) ? check_map_after_distance(&observe_lastpos.point1x, &observe_lastpos.point1y) : - check_map_after_distance(&observe_lastpos.point2x, &observe_lastpos.point2y); + uint8_t mapaccess = (checkside == TRACKLEFT) ? check_map_after_distance(&observe_lastpos.point1) : + check_map_after_distance(&observe_lastpos.point2); // weitere Checks nur wenn gewisse Strecke gefahren oder Endeanforderung besteht if (!endrequest && !mapaccess) @@ -345,22 +445,22 @@ &map_pos); //Endepunktsuche nicht weiter verfolgen falls noch kein Startpunkt vorliegt - if ((observer->point1x == 0 && observer->point1y == 0)) { + 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 - if ((mapval < 0 || mapval >= MAP_TRACKVAL) && *lastpointx == 0 && *lastpointy == 0) { - set_point_to_lastpoint(&observer->point1x, &observer->point1y, 0, 0); + 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(lastpointx, lastpointy, 0, 0); + set_point_to_lastpoint(lastpoint, 0, 0); return False; //Endepunktsuche ungueltig } // Abstand zum Startpunkt muss gueltig sein sonst ist Schluss - if (!(dist_valid(observer->point1x, observer->point1y, map_pos.x, map_pos.y))) { + if (!(dist_valid(observer->point1, map_pos))) { *behavstate = (endrequest) ? TRACK_END : 1;// bleibt in Endepunktsuche wenn nicht Endeanforderung kam return False; //Endepunktsuche ungueltig } @@ -371,29 +471,29 @@ // bei Hindernis auf aktuellem Punkt wird der letzte gemerkte Nicht-Hindernispunkt auf den Stack gespeichert, der // vorher auf Gueltigkeit gecheckt wird, d.h. muss bestimmten Mindestabstand zum Startpunkt haben und kein Hindernis sein - set_point_to_lastpoint(&observer->point2x, &observer->point2y, *lastpointx, *lastpointy); + set_point_to_lastpoint(&observer->point2, lastpoint->x, lastpoint->y); //zuletzt gueltigen Endpunkt auf Stack sichern - push_stack_pos_line(observer->point1x, observer->point1y,*lastpointx, *lastpointy); + push_stack_pos_line(observer->point1, *lastpoint); + LOG_DEBUG("Push Endepunkt Hind or Track Seite %1d P: %1d %1d",checkside,lastpoint->x, lastpoint->y); //letzten gemerkten Punkt wieder als ungueltig kennzeichnen - set_point_to_lastpoint(lastpointx, lastpointy, 0, 0); + set_point_to_lastpoint(lastpoint, 0, 0); } else { //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(lastpointx, lastpointy, map_pos.x, map_pos.y); + set_point_to_lastpoint(lastpoint, map_pos.x, map_pos.y); // falls sich bei diesem gueltigen Endpunkt herausstellt, dass der vorhin ermittelte Startpunkt nun Hinderniswert besitzt, so wird der Endpunkt zum Startpunkt // und weiter mit Endpunktsuche - if (map_get_field(observer->point1x, observer->point1y) < 0) { - set_point_to_lastpoint(&observer->point1x, &observer->point1y, - *lastpointx, *lastpointy); // Startpunkt ueberschreiben + 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 - set_point_to_lastpoint(lastpointx, lastpointy, 0, 0); - set_point_to_lastpoint(&observer->point2x, &observer->point2y, 0, 0); + set_point_to_lastpoint(lastpoint, 0, 0); + set_point_to_lastpoint(&observer->point2, 0, 0); return True; // gueltiger Endepunkt ermittelt } @@ -402,10 +502,12 @@ return False; // gueltigen Endepunkt in Observervar vermerken - set_point_to_lastpoint(&observer->point2x, &observer->point2y, *lastpointx, *lastpointy); + set_point_to_lastpoint(&observer->point2, lastpoint->x, lastpoint->y); //Merken der Strecke bei Endanforderung auf dem Stack - push_stack_pos_line(observer->point1x, observer->point1y, *lastpointx, *lastpointy); + push_stack_pos_line(observer->point1, *lastpoint); + + LOG_DEBUG("Push Endanf. Seite %1d P: %1d %1d",checkside,observer->point1.x,observer->point1.y); } @@ -413,15 +515,14 @@ *behavstate = (endrequest) ? TRACK_END : 0; // solange die Startpunktsuche keinen neuen Startpunkt ermittelt hat, ist dieser initial - set_point_to_lastpoint(&observer->point1x, &observer->point1y, 0, 0); + set_point_to_lastpoint(&observer->point1, 0, 0); return True; // Streckenpunkt ist hier gueltig } /*! Merkvariable des letzten gueltigen Endpunktes fuer Obserververhalten linke Spur */ -static int16_t lastpointleft_x = 0; -static int16_t lastpointleft_y = 0; +static position_t lastpointleft = { 0, 0 }; /*! * Observer links; jeweils ein selbstaendiges Verhalten, welches die Nachbarbahn beobachtet und eine befahrbare Strecke bis zu einem Hindernis @@ -446,15 +547,13 @@ // Anfangspunkt wurde bestimmt und ist hier auf der Suche nach dem Endpunkt; ein Endpunkt wird als Ende der Strecke // 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_x, &lastpointleft_y)) + if (!observe_get_endpoint(CHECK_SIDE, &observe1_state, &observe_trackleft, &lastpointleft)) break; break; default: - if (observe_trackleft.point2x == 0 && observe_trackleft.point2y == 0) - set_point_to_lastpoint(&observe_trackleft.point1x, - &observe_trackleft.point1y, 0, 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; @@ -470,19 +569,16 @@ switch_to_behaviour(caller, bot_observe_left_behaviour, OVERRIDE); observe1_state = 0; endrequest = False; - set_point_to_lastpoint(&observe_trackleft.point1x, - &observe_trackleft.point1y, 0, 0); - set_point_to_lastpoint(&observe_trackleft.point2x, - &observe_trackleft.point2y, 0, 0); - set_point_to_lastpoint(&lastpointleft_x, &lastpointleft_y, 0, 0); - set_point_to_lastpoint(&observe_lastpos.point1x, &observe_lastpos.point1y, 0, 0); + set_point_to_lastpoint(&observe_trackleft.point1, 0, 0); + 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); } } /*! Merkvariable des letzten gueltigen Endpunktes fuer Obserververhalten rechte Spur */ -static int16_t lastpointright_x = 0; -static int16_t lastpointright_y = 0; +static position_t lastpointright = { 0, 0 }; /*! * Observer rechts; jeweils ein selbstaendiges Verhalten, welches die Nachbarbahn beobachtet und eine befahrbare Strecke bis zu einem Hindernis @@ -508,15 +604,13 @@ // Anfangspunkt wurde bestimmt und ist hier auf der Suche nach dem Endpunkt; ein Endpunkt wird als Ende der Strecke // 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_x, &lastpointright_y)) + if (!observe_get_endpoint(CHECK_SIDE, &observe2_state, &observe_trackright, &lastpointright)) break; break; default: - if (observe_trackright.point2x == 0 && observe_trackright.point2y == 0) - set_point_to_lastpoint(&observe_trackright.point1x, - &observe_trackright.point1y, 0, 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; @@ -531,12 +625,10 @@ if (!behaviour_is_activated(bot_observe_right_behaviour)) { switch_to_behaviour(caller, bot_observe_right_behaviour, OVERRIDE); observe2_state = 0; - set_point_to_lastpoint(&observe_trackright.point1x, - &observe_trackright.point1y, 0, 0); - set_point_to_lastpoint(&observe_trackright.point2x, - &observe_trackright.point2y, 0, 0); - set_point_to_lastpoint(&lastpointright_x, &lastpointright_y, 0, 0); - set_point_to_lastpoint(&observe_lastpos.point2x, &observe_lastpos.point2y, 0, 0); + set_point_to_lastpoint(&observe_trackright.point1, 0, 0); + 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); } @@ -579,7 +671,7 @@ /* Check-Routine zum Erkennen eines Hindernisses nach den Abgrund- als auch Abstandssensoren * @return True wenn Hindernis voraus sonst False */ -static uint8_t check_haz_in_map(void) { +static uint8_t check_haz_sensDist(void) { if (border_fired) return True; @@ -600,26 +692,25 @@ * @param y2 Y-Koordinate des Punktes 2 * @param change_points Punkte1 und 2 in nextline werden bei True vertauscht sonst belegt mit den Koordinaten */ -static void set_nextline(int16_t x1, int16_t y1, int16_t x2, int16_t y2, - uint8_t change_points) { +static void set_nextline(position_t point1, position_t point2, uint8_t change_points) { if (change_points) { - int16 xtemp = nextline.point1x; - int16 ytemp = nextline.point1y; - nextline.point1x = nextline.point2x; - nextline.point1y = nextline.point2y; - nextline.point2x = xtemp; - nextline.point2y = ytemp; + int16 xtemp = nextline.point1.x; + int16 ytemp = nextline.point1.y; + nextline.point1.x = nextline.point2.x; + nextline.point1.y = nextline.point2.y; + nextline.point2.x = xtemp; + nextline.point2.y = ytemp; } else { // der Punkt in der Naehe wird richtig nach nextline uebernommen; der naheste nach Punkt1 - if (get_dist(x1, y1, x_pos, y_pos) < get_dist(x2, y2, x_pos, y_pos)) { + 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.point1x, &nextline.point1y, x1, y1); - set_point_to_lastpoint(&nextline.point2x, &nextline.point2y, x2, y2); + set_point_to_lastpoint(&nextline.point1, point1.x, point1.y); + set_point_to_lastpoint(&nextline.point2, point2.x, point2.y); } else { // der Punkt in der Naehe wird richtig nach nextline uebernommen; der naheste nach Punkt1 - set_point_to_lastpoint(&nextline.point1x, &nextline.point1y, x2, y2); - set_point_to_lastpoint(&nextline.point2x, &nextline.point2y, x1, y1); + set_point_to_lastpoint(&nextline.point1, point2.x, point2.y); + set_point_to_lastpoint(&nextline.point2, point1.x, point1.y); } } } @@ -655,8 +746,8 @@ position_t pos; - uint8_t free1; - uint8_t free2; + //uint8_t free1; + //uint8_t free2; deactivateBehaviour(bot_cancel_behaviour_behaviour); // Cancel-Verhalten abbrechen, damit es nicht ewig weiterlaeuft @@ -666,22 +757,21 @@ BLOCK_BEHAVIOUR(data, 500); // etwas warten - // anzufahrende naechste Strecke erst mal initialisieren - set_point_to_lastpoint(&nextline.point1x, &nextline.point1y, 0, 0); - set_point_to_lastpoint(&nextline.point2x, &nextline.point2y, 0, 0); - - - if (check_haz_in_map() || (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 + // TODO: Diese Strecke nicht gleich verwerfen sondern evtl untersuchen und Teilstrecken bilden; evtl. mit Pfadplanung andere Teilstrecke anfahren + // oder erst einmal mit anderem Weg tauschen, der gerade frei ist. + // da jetzt auch eigener Linienstack verwendet wird, koennte man einen Zaehler mitfuehren wie oft eine Strecke schon zurueckgestellt wurde und erst ab + // einem Schwellwert wirklich verwerfen + track_state = GET_LINE_FROM_STACK; + LOG_DEBUG("zu Stackholen wg. Abstand %1d %1d",sensDistL,sensDistR); break; - } + } //naechster Verhaltenszustand nach diesen Pruefungen; notwendig damit Observer bereits //laufen vor direktem Losfahren track_state = GO_FORWARD; - //Observer starten zum Beobachten der Nebenspuren - start_observe_left_right(); //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 @@ -693,16 +783,29 @@ else getpoint_side_dist(0, 250, TRACKLEFT, &pos); //nur etwas voraus Punkt berechnen wenn weiter gesehen + LOG_DEBUG("Abstaende voraus: L/R: %1d %1d",sensDistL,sensDistR); + // Wegfreiheit bis zum Punkt auf Strecke voraus bestimmen; manchmal kein Hind gesehen aber schon in Map - free1 = map_way_free(x_pos, y_pos, pos.x,pos.y); - // ist der Weg voraus nicht frei laut Map, dann gar nicht erst fahren und verwerfen - if (!free1) { + // ist der Weg voraus nicht frei laut Map bis zum berechneten Punkt, dann gar nicht erst fahren + // Pfadplanung bringt hier gar nichts, da hier ja eine Bahn auf gerader Strecke, eben eine Nebenbahhn, befahren werden soll + // Todo: Teilstrecken berechnen bis zum Hindernis selbst und ab Hindernis bis zum eigentlichen Endpunkt + if (!map_way_free(x_pos, y_pos, pos.x,pos.y)) { // rein optisch ist manchmal der Weg trotzdem frei-Mapverschiebung ? + // TODO: Diese Strecke nicht gleich verwerfen sondern evtl untersuchen und Teilstrecken bilden; evtl. mit Pfadplanung andere Teilstrecke anfahren + // oder erst einmal mit anderem Weg tauschen, der gerade frei ist. + // da jetzt auch eigener Linienstack verwendet wird, koennte man einen Zaehler mitfuehren wie oft eine Strecke schon zurueckgestellt wurde und erst ab + // einem Schwellwert wirklich verwerfen track_state = GET_LINE_FROM_STACK; - LOG_DEBUG("Weg versperrt"); + LOG_DEBUG("Weg versperrt zu Zwischenpunkt %1d %1d, Endpunkt %1d %1d",pos.x,pos.y,nextline.point2.x,nextline.point2.y); break; } + // anzufahrende naechste Strecke erst mal initialisieren + set_point_to_lastpoint(&nextline.point2, 0, 0); + set_point_to_lastpoint(&nextline.point1, 0, 0); + //Observer starten zum Beobachten der Nebenspuren + start_observe_left_right(); + break; case GO_FORWARD: //nach bestandenen Pruefungen fuer Bahn voraus nun vorwaertsfahren @@ -716,7 +819,7 @@ bot_goto_obstacle(data, DIST_AWARD_HAZ, 0); //mit hohem Abstand voraus via obstacle //damit auch Abgrund erkannt wird beim Vorfahren; obige Verhalten benutzen beide goto_pos - bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_haz_in_map); + bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_haz_sensDist); break; case DELAY_AFTER_FORWARD: @@ -764,9 +867,9 @@ backward=False; //init. vor dem naechsten Verhaltenszustand //Drehen des Bots zum nahesten Punkt bei Gueltigkeit desselben - if (nextline.point1x != 0 || nextline.point1y != 0) - bot_turn(data, calc_angle_diff(nextline.point1x - x_pos, - nextline.point1y - y_pos)); + if (nextline.point1.x != 0 || nextline.point1.y != 0) + bot_turn(data, calc_angle_diff(nextline.point1.x - x_pos, + nextline.point1.y - y_pos)); break; @@ -779,18 +882,19 @@ //Nach Drehung links und Abstand rechts ist zu klein (oder umgedreht) rueckwaerts fahren; der Punkt nebenan ist etwas vor seitlich und der bot dreht sich //somit zum Punkt aber etwas schraeg und der rechte Sensor ist etwas weiter weg als der linke (oder umgedreht), der bei zu nah unendlich liefert; also der von Wand weitere Sensor //wird zur Pruefung herangezogen, da bei zu nah der andere keinen gueltigen sinnvollen Wert liefert - if (!backward && (sensDistR <= DIST_AWARD_HAZ || sensDistL <= DIST_AWARD_HAZ)) { + if (!backward && (sensDistR <= DIST_AWARD_HAZ || sensDistL <= DIST_AWARD_HAZ)) { //etwas zurueck um Map zu checken - bot_drive_distance(data, 0, -BOT_SPEED_NORMAL, 5); // rueckwaerts bei Abgrund + LOG_DEBUG("zu Nah->Back L: %1d R: %1d",sensDistL,sensDistR); + bot_drive_distance(data, 0, -BOT_SPEED_NORMAL, 5); // rueckwaerts bei Abgrund //direct_choice=0;//beim naechsten Mal immer noch zu nah Stackholen backward=True; // Kennung auf rueckwaerts gefahren setzen, da nur 1x erlaubt break;// in aktuellem Eingang drin bleiben - } + } // Nach Ausrichtung zum Ziel darf kein Hind. in geringem Abstand voraus zu sehen sein - if (check_haz_in_map()) { + if (check_haz_sensDist()) { track_state = GET_LINE_FROM_STACK; - LOG_DEBUG("Hind voraus"); + LOG_DEBUG("Hind voraus L: %1d R: %1d",sensDistL,sensDistR); break; } @@ -798,7 +902,7 @@ track_state = GOTO_FIRST_DEST; //Abstand des bots zum ersten Zielpunkt bestimmen - int16_t dist_to_point = sqrt(get_dist(nextline.point1x, nextline.point1y, x_pos, y_pos)); + int16_t dist_to_point = sqrt(get_dist(nextline.point1.x, nextline.point1.y, x_pos, y_pos)); //Kennung fuer grossen Abstand zum Zielpunkt init. go_long_distance = (dist_to_point>150) ? True : False; @@ -813,7 +917,7 @@ bot_goto_dist(data, dist_to_point - 400, 1); // bis 400mm vor Ziel //damit bot nicht auf seinem weiten Weg zum Zielpunkt in den Abgrund faellt - bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_haz_in_map); + bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_haz_sensDist); } break; @@ -824,7 +928,7 @@ //zum Ziel ist; damit diese Strecke nicht verloren geht, Weg auf Stack nehmen // Bot hat sein Ziel erreicht ohne wegen Hindernis voraus Fahrt abgebrochen zu haben - if (!check_haz_in_map()) { + if (!check_haz_sensDist()) { track_state = GOTO_FIRST_DEST; //naechster Zustand ohne Abbruch break; } @@ -835,12 +939,9 @@ static trackpoint_t p_temp; //zum Merken beim Vertauschen //gibt es noch einen Stackeintrag, so wird die aktuell anzufahrende Strecke (nextline) auf den Stack gelegt und darueber //als gueltige naechste Strecke wieder den letzten Stackeintrag - if (pop_stack_pos_line(&p_temp.point1x, &p_temp.point1y, - &p_temp.point2x, &p_temp.point2y)) { - push_stack_pos_line(nextline.point1x, nextline.point1y, - nextline.point2x, nextline.point2y); - push_stack_pos_line(p_temp.point1x, p_temp.point1y, p_temp.point2x, - p_temp.point2y); + if (/*pop_stack_pos_line*/line_store_pop(&p_temp.point1,&p_temp.point2)) { + push_stack_pos_line(nextline.point1, nextline.point2); + push_stack_pos_line(p_temp.point1, p_temp.point2); } track_state = GET_LINE_FROM_STACK; //weiterfahren mit der naechsten gueltigen Strecke @@ -854,20 +955,19 @@ //falls weit gefahren wurde und Hindernis zum naechsten Zielpunkt erkannt wird aber der andere Streckenpunkt freie Fahrt bietet, dann einfach //Punkte tauschen und den anderen Punkt zuerst anfahren um sich diesem von der anderen Seite zu naehern - if (go_long_distance && ((check_haz_in_map()) || map_get_field( - nextline.point1x, nextline.point1y) < 0) && map_way_free(x_pos, - y_pos, nextline.point2x, nextline.point2y)) - set_nextline(0, 0, 0, 0, True); //Werte egal da nextline nur getauscht + if (go_long_distance && ((check_haz_sensDist()) || map_get_field(nextline.point1) < 0) && map_way_free(x_pos, + y_pos, nextline.point2.x, nextline.point2.y)) + set_nextline(nextline.point1,nextline.point2, True); //Werte nur getauscht //Ersten Streckenpunkt nun anfahren - bot_goto_pos(data, nextline.point1x, nextline.point1y, 999); + bot_goto_pos(data, nextline.point1.x, nextline.point1.y, 999); lastCorrectionTime = 0; //Ablaufzeit init. //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_in_map); // Abgrund und Abstandsensorcheck bei langer Entfernung (nach Stackholen oder rueckwaerts wg. zu nah) + bot_cancel_behaviour(data, bot_goto_pos_behaviour, 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 @@ -886,10 +986,11 @@ BLOCK_BEHAVIOUR(data, 500); // etwas warten //Ausrichten auf den Endpunkt der Fahrstrecke; ebenfalls wieder zum Aktualisieren der Map vor dem Anfahren selbst - bot_turn(data, calc_angle_diff(nextline.point2x - x_pos,nextline.point2y - y_pos)); + bot_turn(data, calc_angle_diff(nextline.point2.x - x_pos,nextline.point2.y - y_pos)); //naechster Verhaltenszustand, welches wiederum der Starteintritt des Verhaltens selbst ist und geht hiermit von vorn los track_state = CHECK_TRACKSIDE; + LOG_DEBUG("Ausrichten auf %1d %1d",nextline.point2.x,nextline.point2.y); break; @@ -900,40 +1001,52 @@ //etwas hier verbleiben, damit Observer sicher gestoppt sind if (!timer_ms_passed(&lastCheckTime, CORRECTION_DELAY + 300)) break; - + // auf jeden Fall hier Stop der Obserververhalten if (!endrequest) bot_stop_observe(); // Weg vom Stack holen und Ende falls leer - if (!pop_stack_pos_line(&nextline.point1x, &nextline.point1y, &nextline.point2x, &nextline.point2y)) { + if (!line_store_pop(&nextline.point1, &nextline.point2)) { LOG_DEBUG("Stack leer"); track_state = TRACK_END; break; } - + // der naheste Punkt zum Bot laut Luftdistance wird nextline - set_nextline(nextline.point1x, nextline.point1y, nextline.point2x, nextline.point2y, 0); + set_nextline(nextline.point1, nextline.point2, 0); + + LOG_DEBUG("Stackholen P1: %1d %1d P2: %1d %1d",nextline.point1.x,nextline.point1.y,nextline.point2.x,nextline.point2.y); //pruefen, ob Wege zu den Punkten frei sind und Freikennungen speichern - free1 = map_way_free(x_pos, y_pos, nextline.point1x, nextline.point1y); - free2 = map_way_free(x_pos, y_pos, nextline.point2x, nextline.point2y); + uint8_t free1 = map_way_free(x_pos, y_pos, nextline.point1.x, nextline.point1.y); + + // 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 + 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); + } + #else + + uint8_t free2 = map_way_free(x_pos, y_pos, nextline.point2.x, nextline.point2.y); - // falls Weg zu keinem Punkt frei, dann mit dem naechsten Stackweg tauschen; ist dieser auch nicht befahrbar, - // wird dieser verworfen und der Weg - if (!free1 && !free2) { + // falls Weg zu keinem Punkt frei, dann mit dem naechsten Stackweg tauschen; ist dieser auch nicht befahrbar, + // wird dieser verworfen und der Weg + if (!free1 && !free2) { LOG_DEBUG("Stackweg nicht frei"); static trackpoint_t pt; //zum Merken beim Vertauschen - if (pop_stack_pos_line(&pt.point1x, &pt.point1y, &pt.point2x, &pt.point2y)) { + if (line_store_pop(&pt.point1, &pt.point2)) { + 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); - free1 = map_way_free(x_pos, y_pos, pt.point1x, pt.point1y); - free2 = map_way_free(x_pos, y_pos, pt.point2x, pt.point2y); - if (!free1 && !free2) { LOG_DEBUG("Folgeweg auch nicht frei"); //Weg verworfen wenn nicht anfahrbar->naechsten Stackweg holen break; @@ -941,13 +1054,15 @@ //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.point1x, nextline.point1y, nextline.point2x, nextline.point2y); //wieder auf Stack - push_stack_pos_line(pt.point1x, pt.point1y, pt.point2x, pt.point2y); //wieder auf Stack + 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 + //naechster Verhaltenszustand zum Anfahren der naechsten Bahn mit Ausrichten auf den naeheren Punkt laut nextline track_state = TURN_TO_NEAREST; break; @@ -955,7 +1070,6 @@ default: // am Ende des Verhaltens Observer stoppen bot_stop_observe(); - pos_store_clear(); // Stack bereinigen return_from_behaviour(data); break; } @@ -971,7 +1085,10 @@ switch_to_behaviour(caller, bot_drive_area_behaviour, OVERRIDE); track_state = CHECK_TRACKSIDE; border_fired = False; - pos_store_clear(); + line_store_clear(); // Stack initialisieren + // naechste anzufahrende naechste Strecke erst mal initialisieren + set_point_to_lastpoint(&nextline.point2, 0, 0); + set_point_to_lastpoint(&nextline.point1, 0, 0); /* Kollisions-Verhalten ausschalten */ #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE Index: C:/eclipse/workspace/ct-Bot/Changelog.txt =================================================================== --- C:/eclipse/workspace/ct-Bot/Changelog.txt (revision 1498) +++ C:/eclipse/workspace/ct-Bot/Changelog.txt (working copy) @@ -1,5 +1,7 @@ CHANGELOG fuer c't-Bot ====================== +2008-10-27 Frank Menzel [Menzelfr@xxxxxxx]: Einbau des Pfadplanungsverhaltens in drive_area + 2008-10-24 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Positionsspeicher um pos_store_get_sp(), pos_store_get_fp() und pos_store_pop_until() ergaenzt. Ermoeglicht pop bis zu einem gegebenen Stackpointer. 2008-10-24 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Bot-2-Bot-Kommunikation gegen ungueltige Kommandos abgesichert, Debug-Ausgaben ausgeschaltet Index: C:/eclipse/workspace/ct-Bot/ct-Bot.h =================================================================== --- C:/eclipse/workspace/ct-Bot/ct-Bot.h (revision 1498) +++ C:/eclipse/workspace/ct-Bot/ct-Bot.h (working copy) @@ -53,7 +53,7 @@ #define MEASURE_MOUSE_AVAILABLE /*!< Geschwindigkeiten werden aus den Maussensordaten berechnet */ //#define MEASURE_COUPLED_AVAILABLE /*!< Geschwindigkeiten werden aus Maus- und Encoderwerten ermittelt und gekoppelt */ -//#define POS_STORE_AVAILABLE /*!< Positionsspeicher vorhanden */ +#define POS_STORE_AVAILABLE /*!< Positionsspeicher vorhanden */ //#define WELCOME_AVAILABLE /*!< kleiner Willkommensgruss */ @@ -70,7 +70,7 @@ #define BEHAVIOUR_AVAILABLE /*!< Nur wenn dieser Parameter gesetzt ist, exisitiert das Verhaltenssystem */ -//#define MAP_AVAILABLE /*!< Aktiviere die Kartographie */ +#define MAP_AVAILABLE /*!< Aktiviere die Kartographie */ //#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 1498) +++ C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h (working copy) @@ -55,7 +55,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 */ /* Aufgrund einer ganzen reihe von Abhaengigkeiten sollte man beim Versuch Speicher * zu sparen, zuerst mal bei den Hauptverhalten ausmisten, sonst kommen die 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 @@ +#Mon Oct 27 16:31:49 CET 2008 +eclipse.preferences.version=1 +indexerId=org.eclipse.cdt.core.fastIndexer