root / trunk / code / projects / object_manipulation / obj_detect_swarm / bfs_fsm.c @ 874
History | View | Annotate | Download (8.62 KB)
1 | 741 | dsschult | #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 |