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
|
/*
* 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_scan.c
* @brief Scannt die Umgebung und traegt sie in die Karte ein
*
* @author Benjamin Benz (bbe@heise.de)
* @date 03.11.06
*/
#include "bot-logic/bot-logik.h"
#include "map.h"
#include <math.h>
#ifdef BEHAVIOUR_SCAN_AVAILABLE
/*!
* Der Roboter faehrt einen Vollkreis und scannt dabei die Umgebung
* @param *data der Verhaltensdatensatz
*/
void bot_scan_onthefly_behaviour(Behaviour_t *data){
#define ONTHEFLY_DIST_RESOLUTION 0.02 /*!< Alle wieviel gefahrene Strecke [m] soll die Karte aktualisiert werden. Achtung er prueft x und y getrennt, daher ist die tatsaechlich zurueckgelegte Strecke im worst case sqrt(2)*ONTHEFLY_DIST_RESOLUTION */
#define ONTHEFLY_ANGLE_RESOLUTION 10 /*!< Alle wieviel Gerad Drehung [m] soll die Karte aktualisiert werden */
#ifdef MAP_AVAILABLE
static float last_x, last_y, last_head;
float diff_x = fabs(x_pos-last_x);
float diff_y = fabs(y_pos-last_y);
// Wenn der Bot faehrt, aktualisieren wir alles
if ((diff_x > ONTHEFLY_DIST_RESOLUTION) ||( diff_y > ONTHEFLY_DIST_RESOLUTION)){
update_map_location(x_pos,y_pos);
update_map(x_pos,y_pos,heading,sensDistL,sensDistR);
last_x=x_pos;
last_y=y_pos;
last_head=heading;
return;
}
float diff_head = fabs(last_head-heading);
// Wenn der bot nur dreht, aktualisieren wir nur die Blickstrahlen
if ( diff_head > ONTHEFLY_ANGLE_RESOLUTION ){
update_map(x_pos,y_pos,heading,sensDistL,sensDistR);
last_head=heading;
}
/* if ((diff_x*diff_x + diff_y*diff_y > ONTHEFLY_DIST_RESOLUTION)||fabs(last_head-heading) > ONTHEFLY_ANGLE_RESOLUTION ){
last_x=x_pos;
last_y=y_pos;
last_head=heading;
print_map();
}
*/
#endif
}
#define BOT_SCAN_STATE_START 0
uint8 bot_scan_state = BOT_SCAN_STATE_START; /*!< Zustandsvariable fuer bot_scan_behaviour */
/*!
* Der Roboter faehrt einen Vollkreis und scannt dabei die Umgebung
* @param *data der Verhaltensdatensatz
*/
void bot_scan_behaviour(Behaviour_t *data){
#define BOT_SCAN_STATE_SCAN 1
#define ANGLE_RESOLUTION 5 /*!< Aufloesung fuer den Scan in Grad */
// static uint16 bot_scan_start_angle; /*!< Winkel, bei dem mit dem Scan begonnen wurde */
static float turned; /*!< Winkel um den bereits gedreht wurde */
static float last_scan_angle; /*!< Winkel bei dem zuletzt gescannt wurde */
float diff;
switch (bot_scan_state){
case BOT_SCAN_STATE_START:
turned=0;
last_scan_angle=heading-ANGLE_RESOLUTION;
bot_scan_state=BOT_SCAN_STATE_SCAN;
break;
case BOT_SCAN_STATE_SCAN:
diff = heading - last_scan_angle;
if (diff < -180)
diff+=360;
if (diff*1.15 >= ANGLE_RESOLUTION){
turned+= diff;
last_scan_angle=heading;
#ifdef MAP_AVAILABLE
// Eigentlicher Scan hier
update_map(x_pos,y_pos,heading,sensDistL,sensDistR);
////////////
#endif
}
if (turned >= 360-ANGLE_RESOLUTION) // Ende erreicht
bot_scan_state++;
break;
default:
bot_scan_state = BOT_SCAN_STATE_START;
#ifdef MAP_AVAILABLE
print_map();
#endif
return_from_behaviour(data);
break;
}
}
/*!
* Der Roboter faehrt einen Vollkreis und scannt dabei die Umgebung
* @param Der aufrufer
*/
void bot_scan(Behaviour_t* caller){
bot_scan_state = BOT_SCAN_STATE_START;
bot_turn(caller,360);
switch_to_behaviour(0, bot_scan_behaviour,OVERRIDE);
// update_map(x_pos,y_pos,heading,sensDistL,sensDistR);
// print_map();
}
#endif
|
