diff options
Diffstat (limited to 'source/ct-Bot/bot-logic/behaviour_follow_line.c')
| -rw-r--r-- | source/ct-Bot/bot-logic/behaviour_follow_line.c | 176 |
1 files changed, 176 insertions, 0 deletions
diff --git a/source/ct-Bot/bot-logic/behaviour_follow_line.c b/source/ct-Bot/bot-logic/behaviour_follow_line.c new file mode 100644 index 0000000..e35b16f --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_follow_line.c @@ -0,0 +1,176 @@ +/* + * c't-Bot + * + * This program is free software; you can redistribute it + * and/or modify it under the terms of the GNU General + * Public License as published by the Free Software + * Foundation; either version 2 of the License, or (at your + * option) any later version. + * This program is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + * You should have received a copy of the GNU General Public + * License along with this program; if not, write to the Free + * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, + * MA 02111-1307, USA. + * + */ + +/*! @file behaviour_follow_line.c + * @brief Linienverfolger + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + +#include "bot-logic/bot-logik.h" + +#ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE + +/*! Folgt einer Linie, sobald beide Liniensensoren ausloesen + * Die Linie sollte in etwa die Breite beider CNY70 haben + */ +void bot_follow_line_behaviour(Behaviour_t *data) { + /* Konstanten fuer das Verhalten */ + #define CORNER_LEFT 1 + #define CORNER_RIGHT 2 + /* Zustaende fuer das Verhalten */ + #define CHECK_LINE 0 /* !< Feststellen ob wir ueber einer Linie sind */ + #define FOLLOW_LINE 1 /* !< Folgen einer geraden oder leicht gekruemmten Linie */ + #define CHECK_BORDER 2 /* !< Abgrundsensoren haben Alarm geschlagen. Feststellen ob wirklich Abgrund oder Ecke */ + #define CORNER_TURN 3 /* !< Drehung in Richtun detektiertem Abgrund */ + #define CORRECT_POS 4 /* !< Nach der Drehung 1cm vorfahren zum Ausgleichen */ + #define ADVANCE_CORNER 5 /* !< Auf die Ecke zufahren, bis die Linie verschwindet */ + #define RETREAT_AND_STOP 6 /* !< Zurueckfahren mit voller Geschwindigkeit, dann Stop und Verhalten verlassen */ + + /* Status- und Hilfsvariablen */ + static int8 lineState=CHECK_LINE; + static int8 cornerDetected=False; + + switch(lineState) { + case CHECK_LINE: /* sind beide Sensoren ueber einer Linie? */ + if (sensLineL>=LINE_SENSE && sensLineR>=LINE_SENSE) { + /* zunaechst alle Hilfsverhalten ausschalten, die den Algorithmus stoeren */ + /* Abgrund- und Kollisions-Verhalten ausschalten */ + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + deactivateBehaviour(bot_avoid_col_behaviour); + #endif + #ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE + deactivateBehaviour(bot_avoid_border_behaviour); + #endif + /* bot_glance() stoert bot_turn() */ + //deactivateBehaviour(bot_glance_behaviour); + /* losfahren und nach FOLLOW_LINE wechseln */ + speedWishLeft=BOT_SPEED_FOLLOW; + speedWishRight=BOT_SPEED_FOLLOW; + lineState=FOLLOW_LINE; + } + break; + + case FOLLOW_LINE: + /* Pruefen, ob die Abgrundsensoren einen Abgrund sehen */ + if (sensBorderL>BORDER_DANGEROUS || sensBorderR>BORDER_DANGEROUS) { + /* Abgrund erkannt, das kann jetzt eine Linie sein oder ein richtiger Abgrund.*/ + if (sensBorderL>BORDER_DANGEROUS && sensBorderR>BORDER_DANGEROUS) { + /* Wenn beidseitig erkannt, koennen wir damit nicht umgehen -> + * Ende des Verhaltens */ + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + lineState=CHECK_LINE; /* Verhaltensstatus zuruecksetzen */ + //LOG_INFO(("Stopp in FOLLOW_LINE")); + return_from_behaviour(data); + break; + } + /* nachsehen, ob der linke oder rechte Liniensensor ohne Kontakt zur Linie ist + * und ggfs. gegensteuern */ + if (sensBorderL>BORDER_DANGEROUS) { + cornerDetected=CORNER_LEFT; + } else { + cornerDetected=CORNER_RIGHT; + } + /* nun zur vermuteten Ecke vorfahren */ + lineState=CHECK_BORDER; + bot_drive_distance(data,0,BOT_SPEED_FOLLOW,3); + break; + } + if (sensLineL<LINE_SENSE && sensLineR>LINE_SENSE) { + /* links von der Linie abgekommen, daher nach rechts drehen */ + //LOG_DEBUG(("Drehe rechts")); + speedWishLeft=BOT_SPEED_FOLLOW; + speedWishRight=-BOT_SPEED_FOLLOW; + } else if (sensLineL>LINE_SENSE && sensLineR<LINE_SENSE) { + /* andersrum, also links drehen */ + //LOG_DEBUG(("Drehe links")); + speedWishLeft=-BOT_SPEED_FOLLOW; + speedWishRight=BOT_SPEED_FOLLOW; + } else if (sensLineL>LINE_SENSE && sensLineR>LINE_SENSE) { + /* noch ueber der Linie, also einfach geradeaus weiter */ + //LOG_DEBUG(("Fahre geradeaus")); + speedWishLeft=BOT_SPEED_FOLLOW; + speedWishRight=BOT_SPEED_FOLLOW; + } + break; + + case CHECK_BORDER: + /* wir sollten jetzt direkt an der Kante zum Abgrund stehen, wenn es + * denn wirklich eine ist. In dem Fall fahren wir ein Stueck zurueck. + * sonst gehen wir von einer Linie aus, drehen uns in die Richtung, + * in der wir den "Abgrund" entdeckt haben und machen dann weiter mit + * der Linienverfolgung */ + if (sensBorderL>BORDER_DANGEROUS || sensBorderR>BORDER_DANGEROUS) { + /* scheint wirklich ein Abgrund zu sein */ + lineState=RETREAT_AND_STOP; + speedWishLeft=-BOT_SPEED_MAX; + speedWishRight=-BOT_SPEED_MAX; + break; + } + /* war nur eine Ecke, noch weiter darauf zu bis kein Kontakt mehr zur Linie */ + lineState=ADVANCE_CORNER; + speedWishLeft=BOT_SPEED_FOLLOW; + speedWishRight=BOT_SPEED_FOLLOW; + break; + + case ADVANCE_CORNER: + /* auf die Ecke zufahren, bis die Linie verschwindet */ + if (sensLineL<LINE_SENSE && sensLineR<LINE_SENSE) { + /* Linie weg, also Stop, kurz zurueck und drehen */ + lineState=CORNER_TURN; + speedWishLeft=-BOT_SPEED_SLOW; + speedWishRight=-BOT_SPEED_SLOW; + break; + } + speedWishLeft=BOT_SPEED_FOLLOW; + speedWishRight=BOT_SPEED_FOLLOW; + break; + + case CORNER_TURN: + /* 90� in Richtung des detektierten Abgrunds drehen */ + lineState=CORRECT_POS; + bot_turn(data,(cornerDetected==CORNER_LEFT)?90:-90); + cornerDetected=False; + break; + + case CORRECT_POS: + lineState=FOLLOW_LINE; + bot_drive_distance(data,0,BOT_SPEED_SLOW,2); + break; + + case RETREAT_AND_STOP: + /* wir sind an einem Abgrund, Stop und Ende des Verhaltens */ + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + cornerDetected=False; + lineState=CHECK_LINE; + return_from_behaviour(data); + break; + } +} + +/*! Folgt einer Linie, sobald beide Liniensensoren ausloesen + * Die Linie sollte in etwa die Breite beider CNY70 haben + */ +void bot_follow_line(Behaviour_t *caller) { + switch_to_behaviour(caller, bot_follow_line_behaviour,NOOVERRIDE); +} +#endif |