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