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