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
|
/*
* 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_avoid_col.c
* @brief Vermeide Kollisionen
*
* @author Benjamin Benz (bbe@heise.de)
* @date 03.11.06
*/
#include "bot-logic/bot-logik.h"
#ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
#define ZONE_CLOSEST 0 /*!< Zone fuer extremen Nahbereich */
#define ZONE_NEAR 1 /*!< Zone fuer Nahbereich */
#define ZONE_FAR 2 /*!< Zone fuer Fernbereich */
#define ZONE_CLEAR 3 /*!< Zone fuer Freien Bereich */
#define BRAKE_CLOSEST -1.0 /*!< Bremsfaktor fuer extremen Nahbereich ( <1 ==> bremsen <0 ==> rueckwaerts) */
#define BRAKE_NEAR 0.6 /*!< Bremsfaktor fuer Nahbereich ( <1 ==> bremsen <0 ==> rueckwaerts) */
#define BRAKE_FAR 0.8 /*!< Bremsfaktor fuer Fernbereich ( <1 ==> bremsen <0 ==> rueckwaerts) */
uint8 col_zone_l=ZONE_CLEAR; /*!< Kollisionszone, in der sich der linke Sensor befindet */
uint8 col_zone_r=ZONE_CLEAR; /*!< Kollisionszone, in der sich der rechte Sensor befindet */
/*!
* Passt auf, dass keine Kollision mit Hindernissen an der Front des Roboters
* geschieht.
* TODO: Diese Funktion ist nur ein Dummy-Beispiel, wie eine Kollisionsvermeidung aussehen
* koennte. Hier ist ein guter Einstiegspunkt fuer eigene Experimente und Algorithmen!
* @param *data der Verhaltensdatensatz
*/
void bot_avoid_col_behaviour(Behaviour_t *data){
if (sensDistR < COL_CLOSEST) /* sehr nah */
col_zone_r=ZONE_CLOSEST; /* dann auf jeden Fall CLOSEST Zone */
else
/* sind wir naeher als NEAR und nicht in der inneren Zone gewesen */
if ((sensDistR < COL_NEAR) && (col_zone_r > ZONE_CLOSEST))
col_zone_r=ZONE_NEAR; /* dann auf in die NEAR-Zone */
else
/* sind wir naeher als FAR und nicht in der NEAR-Zone gewesen */
if ((sensDistR < COL_FAR) && (col_zone_r > ZONE_NEAR))
col_zone_r=ZONE_FAR; /* dann auf in die FAR-Zone */
else
/* wir waren in einer engeren Zone und verlassen sie in Richtung NEAR */
if (sensDistR < (COL_NEAR * 0.50))
col_zone_r=ZONE_NEAR; /* dann auf in die NEAR-Zone */
else
if (sensDistR < (COL_FAR * 0.50))
col_zone_r=ZONE_FAR; /* dann auf in die NEAR-Zone */
else
col_zone_r=ZONE_CLEAR; /* dann auf in die CLEAR-Zone */
if (sensDistL < COL_CLOSEST) /* sehr nah */
col_zone_l=ZONE_CLOSEST; /* dann auf jeden Fall CLOSEST-Zone */
else
/* sind wir naeher als NEAR und nicht in der inneren Zone gewesen */
if ((sensDistL < COL_NEAR) && (col_zone_l > ZONE_CLOSEST))
col_zone_l=ZONE_NEAR; /* dann auf in die NEAR-Zone */
else
/* sind wir naeher als FAR und nicht in der NEAR-Zone gewesen */
if ((sensDistL < COL_FAR) && (col_zone_l > ZONE_NEAR))
col_zone_l=ZONE_FAR; /* dann auf in die FAR-Zone */
else
/* wir waren in einer engeren Zone und verlassen sie in Richtung NEAR */
if (sensDistL < (COL_NEAR * 0.50))
col_zone_l=ZONE_NEAR; /* dann auf in die NEAR-Zone */
else
if (sensDistL < (COL_FAR * 0.50))
col_zone_l=ZONE_FAR; /* dann auf in die NEAR-Zone */
else
col_zone_l=ZONE_CLEAR; /* dann auf in die CLEAR-Zone */
// Nur reagieren, wenn der Bot vorwaerts faehrt
if ((speed_l > 0) && (speed_r >0)) {
// wenn auf beiden Seiten in der Kollisionszone
if ((col_zone_r == ZONE_CLOSEST)&&(col_zone_l == ZONE_CLOSEST)){
// Drehe Dich zur Seite, wo mehr Platz ist
if (sensDistR < sensDistL)
bot_turn(data,20);
else
bot_turn(data,-20);
return;
}
switch (col_zone_l){
case ZONE_CLOSEST:
faktorWishRight = BRAKE_CLOSEST;
break;
case ZONE_NEAR:
faktorWishRight = BRAKE_NEAR;
break;
case ZONE_FAR:
faktorWishRight = BRAKE_FAR;
break;
case ZONE_CLEAR:
faktorWishRight = 1;
break;
default:
col_zone_l = ZONE_CLEAR;
break;
}
switch (col_zone_r){
case ZONE_CLOSEST:
faktorWishLeft = BRAKE_CLOSEST;
break;
case ZONE_NEAR:
faktorWishLeft = BRAKE_NEAR;
break;
case ZONE_FAR:
faktorWishLeft = BRAKE_FAR;
break;
case ZONE_CLEAR:
faktorWishLeft = 1;
break;
default:
col_zone_r = ZONE_CLEAR;
break;
}
}
}
#endif
|
