1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
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
|
