176 lines
6.3 KiB
C
176 lines
6.3 KiB
C
/*
|
||
* 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<39> 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
|