root / trunk / code / projects / object_manipulation / obj_detect_swarm / bfs_fsm.c @ 741
History | View | Annotate | Download (8.62 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wireless.h> |
3 |
#include <wl_token_ring.h> |
4 |
#include "queue.h" |
5 |
#include <string.h> |
6 |
#include "bfs_fsm.h" |
7 |
#include "smart_run_around_fsm.h" |
8 |
#include "orbit_fsm.h" |
9 |
|
10 |
/* Used to find a robot (or other object)
|
11 |
|
12 |
Uses bom and token ring and rangefinders
|
13 |
|
14 |
Assumes:
|
15 |
robots are already in a token ring
|
16 |
*/
|
17 |
|
18 |
|
19 |
/* private function prototypes */
|
20 |
void bfs_evaluate_state(void); |
21 |
int bfs_follow(void); |
22 |
|
23 |
|
24 |
/* init */
|
25 |
void bfs_init(int robot) { |
26 |
range_init(); |
27 |
analog_init(1);
|
28 |
motors_init(); |
29 |
orb_init(); |
30 |
orb_enable(); |
31 |
usb_init(); |
32 |
run_around_init(); |
33 |
|
34 |
/*Start in the start state, BFS_SEEK */
|
35 |
bfs_state = BFS_SEEK; |
36 |
|
37 |
bfs_otherRobot = robot; // set which robot to seek
|
38 |
bfs_my_id = wl_get_xbee_id(); |
39 |
bfs_follow_id = -1;
|
40 |
|
41 |
bfs_pControl=0;
|
42 |
bfs_bom = 0;
|
43 |
bfs_bom_count = 0;
|
44 |
|
45 |
|
46 |
/*Initialize distances to zero.*/
|
47 |
bfs_d1=1000; bfs_d2=1000; bfs_d3=1000; bfs_d4=1000; bfs_d5=1000; |
48 |
|
49 |
} |
50 |
|
51 |
/*The main function, call this to update states as frequently as possible.*/
|
52 |
void bfs_fsm(void) { |
53 |
|
54 |
/* reset some values */
|
55 |
bfs_speed = BFS_STRAIGHT_SPEED; |
56 |
|
57 |
/*The following lines ensure that undefined (-1) values
|
58 |
will not update the distances.*/
|
59 |
int temp;
|
60 |
|
61 |
wl_do(); // update wireless
|
62 |
usb_puts("\n\rwl_do");
|
63 |
usb_puts("|state=");
|
64 |
usb_puti(bfs_state); |
65 |
usb_puts("|bfs_follow=");
|
66 |
usb_puti(bfs_follow_id); |
67 |
usb_puts("|");
|
68 |
|
69 |
|
70 |
if (bfs_state == BFS_SEEK) {
|
71 |
bfs_follow_id = bfs_follow(); |
72 |
wl_do(); // update wireless
|
73 |
usb_puti(bfs_follow_id); |
74 |
if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) {
|
75 |
bfs_state = BFS_FOLLOW; // move to follow state
|
76 |
bfs_d2=1000;
|
77 |
bfs_check_id=0;
|
78 |
} |
79 |
} |
80 |
if (bfs_state == BFS_FOLLOW) {
|
81 |
/* check robot to follow */
|
82 |
if (++bfs_check_id > BFS_CHECK_ID_TIME) {
|
83 |
bfs_check_id=0;
|
84 |
temp = bfs_follow(); |
85 |
wl_do(); |
86 |
if (temp == BFS_NO_VAL) {
|
87 |
bfs_state = BFS_SEEK; |
88 |
return;
|
89 |
} else if (temp != bfs_follow_id) |
90 |
bfs_follow_id = temp; |
91 |
} |
92 |
// get bom reading
|
93 |
temp = wl_token_get_my_sensor_reading(bfs_follow_id); |
94 |
if (temp == -1 || temp > 16) { |
95 |
bfs_bom_count++; |
96 |
if (bfs_bom_count > BFS_MAX_BOM_COUNT) {
|
97 |
bfs_state = BFS_SEEK; |
98 |
bfs_bom = 255;
|
99 |
bfs_bom_count = 0;
|
100 |
} |
101 |
} |
102 |
else {
|
103 |
bfs_bom = temp; |
104 |
|
105 |
// modify bom reading so right is negative, left is positive
|
106 |
if (bfs_bom <= 12) |
107 |
bfs_bom -= 4;
|
108 |
else
|
109 |
bfs_bom -= 20;
|
110 |
|
111 |
bfs_pControl = bfs_bom*4;
|
112 |
usb_puti(bfs_bom); |
113 |
|
114 |
|
115 |
// get range reading for front
|
116 |
temp=range_read_distance(IR2); |
117 |
bfs_d2=(temp == -1) ? bfs_d2 : temp;
|
118 |
usb_puti(bfs_d2); |
119 |
|
120 |
if (bfs_d2 < BFS_SLOW_DISTANCE)
|
121 |
bfs_speed = BFS_SLOW_SPEED; |
122 |
|
123 |
if (bfs_d2 < BFS_ORBIT_DISTANCE) {
|
124 |
if (bfs_follow_id != bfs_otherRobot) {
|
125 |
// check if we can now see a robot that's closer to our goal
|
126 |
temp = bfs_follow(); |
127 |
wl_do(); |
128 |
if (temp == BFS_NO_VAL || temp == bfs_follow_id)
|
129 |
bfs_state = BFS_SEEK; |
130 |
else if (temp != bfs_follow_id) |
131 |
bfs_follow_id = temp; |
132 |
return; // redo this function in light of the state changes |
133 |
} else {
|
134 |
// orbit the robot
|
135 |
bfs_state = BFS_ORBIT; // move to orbit state
|
136 |
orbit_init(bfs_follow_id); |
137 |
bfs_d2=1000;
|
138 |
} |
139 |
} |
140 |
} |
141 |
} |
142 |
if (bfs_state == BFS_ORBIT) {
|
143 |
usb_puts("orbiting\n");
|
144 |
orbit_state = ORBIT_INSERTION; |
145 |
while(orbit_state != ORBIT_STOP)
|
146 |
{ orbit_fsm(); } |
147 |
bfs_state = BFS_STOP; // move to stop state
|
148 |
} |
149 |
if (bfs_state == BFS_STOP && bfs_follow_id != bfs_otherRobot) {
|
150 |
bfs_state = BFS_SEEK; // seek the otherRobot
|
151 |
return;
|
152 |
} |
153 |
|
154 |
// evaluate state
|
155 |
bfs_evaluate_state(); |
156 |
} |
157 |
|
158 |
|
159 |
//Acts on state change.
|
160 |
void bfs_evaluate_state(){
|
161 |
switch(bfs_state){
|
162 |
case(BFS_SEEK): orb_set_color(RED);
|
163 |
// move around
|
164 |
run_around_FSM(); // note: better to just incorporate into this file later one
|
165 |
break;
|
166 |
|
167 |
case(BFS_FOLLOW): orb_set_color(GREEN);
|
168 |
// follow another robot
|
169 |
move(bfs_speed,bfs_pControl); |
170 |
break;
|
171 |
|
172 |
case(BFS_ORBIT): //orb_set_color(BLUE); |
173 |
// orbit a robot
|
174 |
//orbit_fsm();
|
175 |
//move(0,0);
|
176 |
break;
|
177 |
|
178 |
case(BFS_STOP): orb_set_color(YELLOW);
|
179 |
// stop
|
180 |
move(0,0); |
181 |
break;
|
182 |
|
183 |
default: orb_set_color(MAGENTA);
|
184 |
/*Should never get here, so stop.*/
|
185 |
move(0,0); |
186 |
break;
|
187 |
} |
188 |
} |
189 |
|
190 |
/* find a robot to follow using BFS
|
191 |
ported from colonybfs by Felix
|
192 |
*/
|
193 |
int bfs_follow()
|
194 |
{ |
195 |
|
196 |
/* pseudocode for BFS
|
197 |
|
198 |
procedure bfs(v)
|
199 |
q := make_queue()
|
200 |
enqueue(q,v)
|
201 |
mark v as visited
|
202 |
while q is not empty
|
203 |
v = dequeue(q)
|
204 |
process v
|
205 |
for all unvisited vertices v' adjacent to v
|
206 |
mark v' as visited
|
207 |
enqueue(q,v')
|
208 |
*/
|
209 |
|
210 |
Queue* q = queue_create(); |
211 |
|
212 |
//int num_current_robots = wl_token_get_robots_in_ring();
|
213 |
|
214 |
// keep track of which nodes you have visited. Indexed by robot #
|
215 |
// 1 if visited, 0 if not
|
216 |
unsigned char visited_nodes[BFS_MAX_ROBOTS]; |
217 |
// keep track of the distance from the start robot to other robots
|
218 |
// also indexed by robot#
|
219 |
unsigned char node_distances[BFS_MAX_ROBOTS]; |
220 |
// this is the path you take
|
221 |
unsigned char path[BFS_MAX_ROBOTS]; |
222 |
|
223 |
//variables you will need
|
224 |
unsigned char current_node; //current node |
225 |
unsigned char current_neighbour_node; //neighbor to the current node |
226 |
unsigned char current_neighbour_val; //value of that neighbour's sensors |
227 |
unsigned char current_distance; //keep track of current distance to the start |
228 |
|
229 |
unsigned char large_number = 255; |
230 |
|
231 |
unsigned char* add_value; |
232 |
|
233 |
|
234 |
|
235 |
//set visited nodes to all 0
|
236 |
memset(visited_nodes, 0, BFS_MAX_ROBOTS);
|
237 |
|
238 |
//set all the distances to a very large number
|
239 |
//this isn't technically necessary, but it's probably a good thing -- just in case
|
240 |
memset(node_distances, large_number, BFS_MAX_ROBOTS); |
241 |
|
242 |
//set the path to all LARGE_NUMBER as well
|
243 |
memset(path, BFS_NO_VAL, BFS_MAX_ROBOTS); |
244 |
|
245 |
//queue_remove_all(q);
|
246 |
|
247 |
//add the start node
|
248 |
add_value = (unsigned char*)malloc(sizeof(char)); |
249 |
(*add_value) = bfs_my_id; |
250 |
queue_add(q, add_value); |
251 |
visited_nodes[bfs_my_id] = 1;
|
252 |
node_distances[bfs_my_id] = 0;
|
253 |
|
254 |
while(!queue_is_empty(q)){
|
255 |
add_value = queue_remove(q); |
256 |
current_node = (*add_value); |
257 |
|
258 |
//get the current distance from you to the start
|
259 |
current_distance = node_distances[current_node]; |
260 |
|
261 |
//this node is on your 'path'
|
262 |
path[current_distance] = current_node; |
263 |
//note: it's OK if this path leads nowhere -- the path will be
|
264 |
//overwritten by a node that does lead to the goal
|
265 |
//(if there is no path, we set path to null anyways)
|
266 |
|
267 |
//from now on, all further nodes will be one further away -- increment
|
268 |
current_distance++; |
269 |
|
270 |
|
271 |
//process node -- basically check if it's the goal
|
272 |
if(current_node == bfs_otherRobot) {
|
273 |
//you reach the goal
|
274 |
//return the first robot in the path, which is the one
|
275 |
//to follow
|
276 |
/*
|
277 |
lcd_putchar('.');
|
278 |
lcd_putchar('.');
|
279 |
for(i = 0; i < MAX_ROBOTS; i++)
|
280 |
lcd_putchar(path[i] +48); //print out the path for fun -- remove this later
|
281 |
lcd_putchar('.');
|
282 |
lcd_putchar('.');
|
283 |
*/
|
284 |
return path[1]; //path[0] is you. path[1] is who you want to follow |
285 |
} |
286 |
|
287 |
// go through all nodes adjacent to current
|
288 |
// in effect, this means going through the current node's row in the matrix
|
289 |
|
290 |
wl_token_iterator_begin(); |
291 |
while(wl_token_iterator_has_next()) {
|
292 |
//the robot # is actually just the index
|
293 |
current_neighbour_node = wl_token_iterator_next(); |
294 |
//the value is what is stored in the matrix (this is important)
|
295 |
current_neighbour_val = wl_token_get_sensor_reading(current_node,current_neighbour_node); |
296 |
|
297 |
//check for connected-ness and that it was not visited
|
298 |
// unconnected visited
|
299 |
if( (current_neighbour_val == BFS_NO_VAL) || (visited_nodes[current_neighbour_node]) ) {
|
300 |
//if it is either unconnected or previously visited, exit
|
301 |
continue; //go to the next node |
302 |
} |
303 |
|
304 |
//update the distance from the start node to this node
|
305 |
node_distances[current_neighbour_node] = current_distance; |
306 |
//also mark it as visited
|
307 |
visited_nodes[current_neighbour_node] = 1;
|
308 |
|
309 |
//also enqueue each neighbour
|
310 |
add_value = (unsigned char*)malloc(sizeof(char)); |
311 |
(*add_value) = current_neighbour_node; |
312 |
queue_add(q, add_value); |
313 |
|
314 |
} |
315 |
} |
316 |
|
317 |
//if we get to here, there is no path
|
318 |
memset(path, 0, BFS_MAX_ROBOTS);
|
319 |
return BFS_NO_VAL;
|
320 |
|
321 |
} |
322 |
|
323 |
|