Absender: Frank Menzel
Datum: Mi, 19.11.2008 17:20:58
In-reply-to:
<BAD0F785-FFD4-4C06-ABA1-5117CA8A3AC8@xxxxxxxxxxxxxxx>
Hallo Timo, habe jetzt alles wieder eingebaut, also die Pfadplanung in das area-Verhalten sowie die durchgängigere Verwendung des Types position_t. Der Stack bzw. Positionsspeicher funktioniert prima. Gruß, Frank -----Ursprüngliche Nachricht----- Von: ct-bot-entwickler-bounces@xxxxxxxxxxxxxxxxx [mailto:ct-bot-entwickler-bounces@xxxxxxxxxxxxxxxxx] Im Auftrag von Timo Sandmann Gesendet: Montag, 3. November 2008 16:57 An: Entwicklung rund um den c't-bot Betreff: Re: [ct-bot] Pfadplanung in area Hallo Frank, könntest du das vielleicht noch mal in den neuen Code integrieren? Der Patch passt jetzt leider gar nicht mehr nach den Code-Updates. Das Problem mit dem Positionsspeicher ist auf jeden Fall schon mal gelöst. Grüße, Timo Am 27.10.2008 um 16:42 schrieb Frank Menzel: > 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 > > <area_pathplaning.txt>_______________________________________________ > ct-bot-entwickler Mailingliste > ct-bot-entwickler@xxxxxxxxxxxxxxxxx > http://www.heise.de/bin/newsletter/listinfo/ct-bot-entwickler _______________________________________________ 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_drive_area.c
===================================================================
--- C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c (revision 1521)
+++ C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_area.c (working copy)
@@ -43,9 +43,23 @@
#ifdef BEHAVIOUR_DRIVE_AREA_AVAILABLE
-//#define DEBUG_DRIVE_AREA // Schalter fuer Debug-Code
+#define DEBUG_BEHAVIOUR_AREA // Schalter fuer Debug-Code
//#define DEBUG_DRIVE_AREA_TIMES // Schalter fuer Zeitmessungen
+
+#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
@@ -91,10 +105,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 */
@@ -124,95 +136,75 @@
/*!
* 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_TIMES
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 Koordinaten des ersten Punktes der Linie
+ * @param point2 Koordinaten 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
- position_t pos;
- pos.x = x1;
- pos.y = y1;
- pos_store_push(pos_store, pos);
- pos.x = x2;
- pos.y = y2;
- pos_store_push(pos_store, pos);
+ // Push der Linien-Einzelpunkte
+ pos_store_push(pos_store, point1);
+ pos_store_push(pos_store, 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
+ * @param *point1 Koordinaten des ersten Punktes der Linie
+ * @param *point2 Koordinaten 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) {
- position_t pos;
- if (!pos_store_pop(pos_store, &pos)) {
+static uint8_t pop_stack_pos_line(position_t *point1, position_t *point2) {
+
+ if (!pos_store_pop(pos_store, point1))
return False;
- }
- *x1 = pos.x;
- *y1 = pos.y;
- if (!pos_store_pop(pos_store, &pos)) {
+ if (!pos_store_pop(pos_store, point2))
return False;
- }
- *x2 = pos.x;
- *y2 = pos.y;
+
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);
}
/*!
@@ -223,8 +215,7 @@
* @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(
@@ -233,10 +224,10 @@
// Rueckgabe des Mapwertes
#ifdef DEBUG_DRIVE_AREA_TIMES
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
}
@@ -288,8 +279,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.x, &observe_lastpos.point1.y) :
+ check_map_after_distance(&observe_lastpos.point2.x,&observe_lastpos.point2.y);
// Schluss bei Endeanforderung oder noch nicht weit genug gefahren
if (endrequest || !mapaccess) {
@@ -307,8 +298,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;
@@ -326,12 +317,11 @@
* @param checkside Seite der zu beobachtenden Nebenbahn (TRACKLEFT TRACKRIGHT)
* @param *behavstate Neuer Zustand des Obserververhalten
* @param *observer Zeiger auf Punkte der Nebenbahn, gueltiger Endepunkt wird hier vermerkt
- * @param *lastpointx letzter gueltiger X-Wert der Nebenbahn, verwendeter Wert bei Erkennung Hindernis
- * @param *lastpointy letzter gueltiger Y-Wert der Nebenbahn, verwendeter Wert bei Erkennung Hindernis
+ * @param *lastpoint Zeiger auf letzten gueltigen Wert der Nebenbahn, verwendeter Wert bei Erkennung Hindernis
* @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;
@@ -340,8 +330,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.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)
@@ -352,22 +342,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
}
@@ -378,29 +368,28 @@
// 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);
//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
}
@@ -409,10 +398,10 @@
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);
}
@@ -420,15 +409,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
@@ -454,14 +442,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_x, &lastpointleft_y))
+ &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;
@@ -477,19 +464,16 @@
activateBehaviour(caller, bot_observe_left_behaviour);
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
@@ -516,14 +500,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_x, &lastpointright_y))
+ &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;
@@ -538,12 +521,10 @@
if (!behaviour_is_activated(bot_observe_right_behaviour)) {
activateBehaviour(caller, bot_observe_right_behaviour);
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);
}
@@ -586,7 +567,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;
@@ -601,32 +582,29 @@
/*!
* Hilfsroutine, welche den Fahrtrack nextline mit den Koordinaten belegt; die Koordinaten
* koennen hierbei vertauscht werden oder in point1x/y die zum Bot nahesten Werte gespeichert
- * @param x1 X-Koordinate des Punktes 1
- * @param y1 Y-Koordinate des Punktes 1
- * @param x2 X-Koordinate des Punktes 2
- * @param y2 Y-Koordinate des Punktes 2
+ * @param point1 Koordinaten des Punktes 1
+ * @param point2 Koordinaten 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);
}
}
}
@@ -657,13 +635,12 @@
void bot_drive_area_behaviour(Behaviour_t * data) {
static uint8_t go_long_distance = 0; //Kennung ob nach Stackholen weit gefahren wurde
static uint32_t lastCheckTime = 0;
- static uint8_t backward = False; // Kennung auf Rueckwaerts gefahren wegen zu nah
static uint16_t disttodrive = 0; //Abstand fuer das Vorwaertsfahren
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
@@ -673,23 +650,12 @@
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
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(data);
-
//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
@@ -700,15 +666,32 @@
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);
+ uint8_t 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) {
+ // 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;
- }
+ }
+
+ //naechster Verhaltenszustand nach diesen Pruefungen; notwendig damit Observer bereits
+ //laufen vor direktem Losfahren
+ track_state = GO_FORWARD;
+
+ // anzufahrende naechste Strecke initialisieren
+ set_point_to_lastpoint(&nextline.point1, 0, 0);
+ set_point_to_lastpoint(&nextline.point2, 0, 0);
+
+ // Observer starten zum Beobachten der Nebenspuren
+ start_observe_left_right(data);
break;
@@ -723,7 +706,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:
@@ -768,12 +751,10 @@
//naechster Verhaltenszustand
track_state = ONWALL_TO_TRACK;
- 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;
@@ -786,18 +767,17 @@
//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)) {
- //etwas zurueck um Map zu checken
- 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
+ if (sensDistR <= DIST_AWARD_HAZ || sensDistL <= DIST_AWARD_HAZ) {
+ //etwas zurueck um Map zu checken
+ bot_drive_distance(data,0,-BOT_SPEED_NORMAL,2); // rueckwaerts bei Abgrund
+ LOG_DEBUG("zu Nah->Back L: %1d R: %1d, ist < %1d",sensDistL,sensDistR,DIST_AWARD_HAZ);
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;
}
@@ -805,7 +785,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;
@@ -820,7 +800,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;
@@ -831,7 +811,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;
}
@@ -842,12 +822,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(&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
@@ -861,20 +838,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 egal da nextline 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
@@ -893,11 +869,12 @@
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;
case GET_LINE_FROM_STACK:
@@ -913,48 +890,64 @@
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 (!pop_stack_pos_line(&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);
+ 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");
+ LOG_DEBUG("Stackweg nicht frei");
- static trackpoint_t pt; //zum Merken beim Vertauschen
+ static trackpoint_t pt; //zum Merken beim Vertauschen
- if (pop_stack_pos_line(&pt.point1x, &pt.point1y, &pt.point2x, &pt.point2y)) {
+ if (pop_stack_pos_line(&pt.point1, &pt.point2)) {
- free1 = map_way_free(x_pos, y_pos, pt.point1x, pt.point1y);
- free2 = map_way_free(x_pos, y_pos, pt.point2x, pt.point2y);
+ 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);
- if (!free1 && !free2) {
- LOG_DEBUG("Folgeweg auch nicht frei"); //Weg verworfen wenn nicht anfahrbar->naechsten Stackweg holen
- break;
- }
+ 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.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
- LOG_DEBUG("Weg mit Folgeweg getauscht");
- 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
+
//naechster Verhaltenszustand zum Anfahren der naechsten Bahn mit Ausrichten auf den naeheren Punkt laut nextline
track_state = TURN_TO_NEAREST;
break;
@@ -962,7 +955,6 @@
default:
// am Ende des Verhaltens Observer stoppen
bot_stop_observe();
- pos_store_release(pos_store); // Positionsspeicher bereinigen
return_from_behaviour(data);
break;
}
Index: C:/eclipse/workspace/ct-Bot/Changelog.txt
===================================================================
--- C:/eclipse/workspace/ct-Bot/Changelog.txt (revision 1521)
+++ C:/eclipse/workspace/ct-Bot/Changelog.txt (working copy)
@@ -1,5 +1,7 @@
CHANGELOG fuer c't-Bot
======================
+2008-11-19 Frank Menzel [Menzelfr@xxxxxxx] : Einbau der Pfadplanung und des Positionstyps in behaviour_drive_area
+
2008-11-18 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Bugfix fuer bot_goto_dist(), rueckwaerts klappt wieder
2008-11-03 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Verhalten koennen mit activateBehaviour() (oder switch_to_behaviour()) im Hintergrund ausgefuehrt werden (ging vorher auch schon, aber jetzt kann der behaviour-Code auch korrekt damit umgehen),
Index: C:/eclipse/workspace/ct-Bot/ct-Bot.h
===================================================================
--- C:/eclipse/workspace/ct-Bot/ct-Bot.h (revision 1521)
+++ 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 1521)
+++ C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h (working copy)
@@ -33,7 +33,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? */
@@ -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 @@
+#Wed Nov 19 17:02:46 CET 2008
+eclipse.preferences.version=1
+indexerId=org.eclipse.cdt.core.fastIndexer