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