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