Absender: Frank Menzel
Datum: Mi, 25.02.2009 11:59:15
Index: C:/eclipse/workspace/ct-Bot/bot-logic/bot-logik.c
===================================================================
--- C:/eclipse/workspace/ct-Bot/bot-logic/bot-logik.c (revision 1552)
+++ C:/eclipse/workspace/ct-Bot/bot-logic/bot-logik.c (working copy)
@@ -288,6 +288,11 @@
insert_behaviour_to_list(&behaviour, new_behaviour(71, bot_calc_wave_behaviour, INACTIVE));
#endif
+ #ifdef BEHAVIOUR_FOLLOW_LINE_ENHANCED_AVAILABLE
+ // erweiterter Linienfolge, der mit Unterbrechungen und Hindernissen klarkommt
+ insert_behaviour_to_list(&behaviour, new_behaviour(71, bot_follow_line_enhanced_behaviour, INACTIVE));
+ #endif
+
#ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE
// Verhalten um einer Linie zu folgen
insert_behaviour_to_list(&behaviour, new_behaviour(70, bot_follow_line_behaviour, INACTIVE));
Index: C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_follow_line.c
===================================================================
--- C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_follow_line.c (revision 1552)
+++ C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_follow_line.c (working copy)
@@ -31,6 +31,7 @@
#include "bot-logic/bot-logik.h"
#ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE
+#include "math_utils.h"
//#define DEBUG_BEHAVIOUR_FOLLOW_LINE // Schalter fuer Debug-Code
@@ -421,4 +422,184 @@
}
#endif // VERSION
+
+
+#ifdef BEHAVIOUR_FOLLOW_LINE_ENHANCED_AVAILABLE
+
+// Merkposition nach Umrunden Hindernis; erst wieder gewisse Strecke fahren bis Linienfolger abgebrochen werden kann fuer Hindernis
+static position_t oldpos;
+
+// Statusvariable des Verhaltens
+static uint8_t line_state_enh=0;
+
+// naechster Verhaltenseinsprung nach cancel
+static uint8_t next_linestate=0;
+
+// Var fuer Positionsberechnung nach Unterbrechung
+position_t point;
+
+// Verhaltenszustaende
+#define START_ENH 0
+#define START_LINE_ENH 1
+#define CHECK_BREAK_IN_LINE 2
+#define START_SOLVE_MAZE 3
+#define TURN_AROUND_HAZARD 4
+#define GO_AROUND_HAZARD 5
+#define OTHER_SIDE_ON_LINE 6
+#define TURN_NEXT_PART 7
+#define SEARCH_LINE 8
+#define END_LINE_ENH 99
+
+/* Check-Routine zum Erkennen ob sich bot schon auf der Linie befindet
+ * @return True wenn Linie erkannt wurde
+ */
+static uint8_t check_line_sensors(void) {
+ if (sensLineL >= LINE_SENSE || sensLineR >= LINE_SENSE) {
+ line_state_enh = next_linestate; // jeweils naechster Eintrittspunkt
+ return True;
+ }
+ return False;
+}
+
+/* Check-Routine zum Erkennen ob sich linker Liniensensor allein schon auf der Linie befindet; dies waere
+ * ja optimal fuer den Linienfolger, da dieser ja auf linker Kante faehrt; wird verwendet nach Umrunden des
+ * Hindernisses, um sich wieder auf Linie in richtiger Richtung, also vom Hindernis weg, auszurichten
+ * @return True wenn linker Liniensensor allein auf Linie steht
+ */
+static uint8_t check_left_line_sensor(void) {
+ if (sensLineL >= LINE_SENSE && sensLineR <LINE_SENSE)
+ return True;
+ return False;
+}
+
+/* Check-Routine zum Erkennen, ob sich bot an einer Linienunterberechung befindet oder Hindernis gesehen wird; wird
+ * verwendet zum Abbrechen des Linienfolgers an Unterbrechung oder Hindernis
+ * @return True wenn Unterbrechung erkannt wird oder Hindernis voraus gesehen wird
+ */
+static uint8_t check_sensors(void) {
+ // Unterbrechung wird daran erkannt, wenn sich der Linienfolger wegen keiner Linie mehr voraus nach links dreht und dabei
+ // der rechte Abgrundsensor die unterbrochene Linie erkennt; Unterbrechung darf auch nur ca. maximal 3cm sein
+ if (sensBorderR >= BORDER_DANGEROUS && sensLineL<LINE_SENSE && sensLineR<LINE_SENSE) {
+ line_state_enh=CHECK_BREAK_IN_LINE;
+ return True;
+ }
+
+ // Abstand seit letztem Check ermitteln; nur bei Ueberschreitung des Mindestabstandes seit letzter Hindernisumrundung
+ // darf wegen Hindernis abgebrochen werden, da sonst nach Umrundung und gleichem Hindernis beim Ausrichten des Linienfolgers
+ // dieser sonst gleich wieder moeglicherweise abgebrochen wird
+ uint16_t diff = get_dist(x_pos, y_pos, oldpos.x, oldpos.y);
+ if ((diff >= 100 * 100) || (oldpos.x==0 && oldpos.y==0)) {
+ // Merkpos auf 0, wenn Abstand ueberschritten wurde
+ oldpos.x=0;
+ oldpos.y=0;
+ if (sensDistL <= OPTIMAL_DISTANCE || sensDistR <=OPTIMAL_DISTANCE) {
+ line_state_enh=TURN_AROUND_HAZARD;
+ return True;
+ }
+ }
+
+ return False;
+}
+
+
+
+/*! erweiterter Linienfolger, der auch Linienunterbrechungen und Hindernisse handhabt, waehrend der Bot die Linie verfolgt;
+ * die Linienunterbrechung darf nur relativ klein sein (~3cm), so dass sich beim drehen am Ende der Linie der rechte Abgrundsensor
+ * ueber den Neubeginn der unterbrochenen Linie drehn muss
+ * @param *data Verhaltensdatensatz
+ */
+void bot_follow_line_enhanced_behaviour(Behaviour_t * data) {
+ switch (line_state_enh) {
+ case START_ENH: // maximal 60cm vorwaerts bis Linie voraus
+ bot_goto_dist(data, 600, 1);
+ next_linestate=START_LINE_ENH; // Eintritt nachdem Cancel Linie gefunden hat
+ bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_line_sensors); // Ende der Vorwaertsfahrt wenn Linie erkannt wurde
+ line_state_enh=END_LINE_ENH; // Ende falls keine Linie voraus gefunden wird
+ break;
+
+ case START_LINE_ENH: // Einsprung falls sich bot nun auf Linie befindet, weiter mit Linienfolger
+ bot_follow_line(data); // Linienfolger laeuft los
+ bot_cancel_behaviour(data, bot_follow_line_behaviour, check_sensors); // Abbruch Linienfolger bei Unterbrechung oder Hindernis
+ line_state_enh=END_LINE_ENH; // einfach mal auf Ende, bei Cancel Linienfolger wird dort Einsprung gesetzt
+ break;
+
+ case CHECK_BREAK_IN_LINE: // Einsprung falls Linienunterbrechung erkannt wurde
+ // Linie geht ja weiter nach Unterbrechung genau dort, wo rechter Abgrundsensor sich befindet; dort also Punkt berechnen und anfahren
+ point = calc_point_in_distance(heading, BORDERSENSOR_POS_FW - 10, -BORDERSENSOR_POS_SW);
+ bot_goto_pos(data, point.x, point.y, 999);
+ line_state_enh=START_LINE_ENH; // Linienfolger sollte hier wieder greifen nach Ueberwindung der Unterbrechung
+ break;
+
+ case TURN_AROUND_HAZARD: // auf jeden Fall erst mal in Ausgangsrichtung drehen
+ //ToDo: beim Drehen in beide Richtungen ermitteln, ob schon Kante, also Ende, des Hindernisses (Stein auf Linie) gesehen wird und
+ //weil umgehen in diese Richtung kuerzer ist, dortlang drehen und Hindernisumgeher starten; solve_maze geht leider nur wenn bot sich
+ // hier links dreht und dann rechts die Wand hat
+ bot_turn(data, 70); //Links drehen
+ line_state_enh=GO_AROUND_HAZARD; // naechsten Einsprung setzen
+ break;
+
+ case GO_AROUND_HAZARD: // etwas vorwaerts fahren um von Linie wegzukommen
+ //ToDo_beim Drehen in beide Richtungen ermitteln, ob schon Kante, also Ende, des Hindernisses (Stein auf Linie) gesehen wird und
+ //weil umgehen in diese Richtung kuerzer ist, dortlang drehen und Hindernisumgeher starten
+ bot_goto_dist(data,50,1); // etwas vorwaerts fahren um von Linie wegzukommen
+ line_state_enh=START_SOLVE_MAZE; // naechster Einsprung: Start solve_maze zum Umrunden des Hindernisses
+ break;
+
+ case START_SOLVE_MAZE: // Start Verhalten solve_maze zum Umrunden des Hindernisses
+ bot_solve_maze(data); // Start solve_maze
+ next_linestate=OTHER_SIDE_ON_LINE; // Eintritt nachdem Cancel Linie erkannt hat
+ // Solve_maze abbrechen, wenn wieder Linie hinter Hindernis erkannt wird
+ bot_cancel_behaviour(data, bot_solve_maze_behaviour, check_line_sensors);
+ break;
+
+
+ case OTHER_SIDE_ON_LINE: // Bot steht auf anderer Seite des Hindernisses wieder auf der Linie
+ // hier muss der bot in die vom Hindernis wegweisende Richtung ausgerichtet werden
+ // da er links um den Stein gegangen ist, blickt er entweder etwas Richtung Stein oder fast parallel;
+ //einfach links wegdrehen, um ungefaehr die richtige Richtung einzunehmen
+
+ //Sicherheit einbauen, dass er beim sich auf Linie Ausrichten und Hindernis sehen nicht wieder cancelt
+ //Pos merken und erst wieder nach gewissem Abstand Hinderniserkennung mit Abbruch Linienfolger erlauben
+ oldpos.x=x_pos;
+ oldpos.y=y_pos;
+ line_state_enh=TURN_NEXT_PART; // naechster Einsprung weiterdrehen mit Cancelueberwachung
+ bot_turn(data,40); // etwas links drehen
+ break;
+
+ case TURN_NEXT_PART: // weitere Drehung in richtiger Richtung fuer Linienfolger mit Cancelueberwachung
+ //zweiter Teil der Drehung, hier aber Abbruch wenn linker Sensor auf Linie, da dies dann optimal fuer den Linienfolger ist
+ bot_turn(data,20);
+ bot_cancel_behaviour(data, bot_turn_behaviour, check_left_line_sensor); // Abbruch Drehung wenn optimal fuer Linienfolger
+ line_state_enh=SEARCH_LINE; // naechster Eintrittspunkt
+ break;
+
+ case SEARCH_LINE: //etwas vorfahren, um optimale Position fuer Linienfolger zu erreichen, falls er noch nicht optimal steht
+ if (!check_left_line_sensor()) {
+ bot_goto_dist(data,50,1); // etwas vor bis nur links Linie erkannt wird, dann optimal fuer Linienfolger
+ bot_cancel_behaviour(data, bot_goto_pos_behaviour, check_left_line_sensor); // Vorfahren abbrechen wenn optimal fuer Linienfolger
+ }
+ //eigentlich wurde hier alles getan, um den bot optimal fuer den Linienfolger hinzusetezn in die richtige Richtung nach Hindernisumrundung; mit dieser
+ //Hoffnung gehts weiter mit dem Linienfolger
+ line_state_enh=START_LINE_ENH; // naechster Einsprung->Linienfolger geht von vorn los
+ break;
+
+ default:
+ return_from_behaviour(data);
+ break;
+ }
+
+}
+
+/*! Botenverhalten fuer den erweiterten Linienfolger
+ * @param *caller Verhaltensdatensatz des Aufrufers
+ */
+void bot_follow_line_enhanced(Behaviour_t * caller) {
+ switch_to_behaviour(caller, bot_follow_line_enhanced_behaviour, NOOVERRIDE);
+ line_state_enh=0;
+ oldpos.x=0;
+ oldpos.y=0;
+}
+
+#endif
+
#endif // BEHAVIOUR_FOLLOW_LINE_AVAILABLE
Index: C:/eclipse/workspace/ct-Bot/Changelog.txt
===================================================================
--- C:/eclipse/workspace/ct-Bot/Changelog.txt (revision 1552)
+++ C:/eclipse/workspace/ct-Bot/Changelog.txt (working copy)
@@ -1,5 +1,7 @@
CHANGELOG fuer c't-Bot
======================
+2009-02-25 Frank Menzel [Menzelfr@xxxxxx]: neues Verhalten behaviour_follow_line_enhanced; Linienfolger um auch mit Unterbrechungen und Hindernissen klarzukommen
+
2009-02-24 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Command-Code optimiert
Bugfix fuer HW-SPI und MOUSE_AVAILABLE (#180)
SPI-Code unterstuetzt verschiedene Geschwindigkeiten
Index: C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h
===================================================================
--- C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h (revision 1552)
+++ C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h (working copy)
@@ -30,6 +30,7 @@
#define BEHAVIOUR_SCAN_AVAILABLE /*!< Gegend scannen vorhanden? */
//#define BEHAVIOUR_SOLVE_MAZE_AVAILABLE /*!< Wandfolger vorhanden? */
#define BEHAVIOUR_FOLLOW_LINE_AVAILABLE /*!< Linienfolger vorhanden? */
+#define BEHAVIOUR_FOLLOW_LINE_ENHANCED_AVAILABLE /*!< erweiterter Linienfolger, der auch mit Unterbrechungen und Hindernis klarkommt */
#define BEHAVIOUR_SERVO_AVAILABLE /*!< Kontrollverhalten fuer die Servos */
@@ -125,6 +126,13 @@
#undef BEHAVIOUR_DRIVE_STACK_AVAILABLE
#endif
+#ifdef BEHAVIOUR_FOLLOW_LINE_ENHANCED_AVAILABLE
+ #define BEHAVIOUR_FOLLOW_LINE_AVAILABLE
+ #define BEHAVIOUR_GOTO_POS_AVAILABLE
+ #define BEHAVIOUR_CANCEL_BEHAVIOUR_AVAILABLE
+ #define BEHAVIOUR_SOLVE_MAZE_AVAILABLE
+#endif
+
#ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE
#define BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE
#define BEHAVIOUR_TURN_AVAILABLE
Index: C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_follow_line.h
===================================================================
--- C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_follow_line.h (revision 1552)
+++ C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_follow_line.h (working copy)
@@ -51,5 +51,19 @@
* @param *caller Verhaltensdatensatz des Aufrufers
*/
void bot_follow_line(Behaviour_t * caller);
+
+
+/*! erweiterter Linienfolger, der auch Linienunterbrechungen und Hindernisse handhabt, waehrend der Bot die Linie verfolgt;
+ * die Linienunterbrechung darf nur relativ klein sein (~3cm), so dass sich beim drehen am Ende der Linie der rechte Abgrundsensor
+ * ueber den Neubeginn der unterbrochenen Linie drehn muss
+ * @param *data Verhaltensdatensatz
+ */
+void bot_follow_line_enhanced_behaviour(Behaviour_t * data);
+
+/*! Botenverhalten fuer den erweiterten Linienfolger
+ * @param *caller Verhaltensdatensatz des Aufrufers
+ */
+void bot_follow_line_enhanced(Behaviour_t * caller);
+
#endif // BEHAVIOUR_FOLLOW_LINE_AVAILABLE
#endif /*BEHAVIOUR_FOLLOW_LINE_H_*/
Index: C:/eclipse/workspace/ct-Bot/ui/rc5.c
===================================================================
--- C:/eclipse/workspace/ct-Bot/ui/rc5.c (revision 1552)
+++ C:/eclipse/workspace/ct-Bot/ui/rc5.c (working copy)
@@ -152,7 +152,9 @@
case 4: bot_follow_object(NULL); break;
#endif // BEHAVIOUR_CATCH_PILLAR_AVAILABLE
- #if defined BEHAVIOUR_SOLVE_MAZE_AVAILABLE
+ #if defined BEHAVIOUR_FOLLOW_LINE_ENHANCED_AVAILABLE
+ case 5: bot_follow_line_enhanced(NULL); break;
+ #elif defined BEHAVIOUR_SOLVE_MAZE_AVAILABLE
case 5: bot_solve_maze(NULL); break;
#elif defined BEHAVIOUR_FOLLOW_LINE_AVAILABLE
case 5: bot_follow_line(NULL); break;
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 Feb 25 11:19:09 CET 2009
+eclipse.preferences.version=1
+indexerId=org.eclipse.cdt.core.fastIndexer