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 connectedness 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 