root / branches / lemmings / code / behaviors / smart_run_around_fsm / lemmings.c @ 204
History | View | Annotate | Download (4.53 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wl_defs.h> |
3 |
#include <wireless.h> |
4 |
#include <wl_token_ring.h> |
5 |
#include "smart_run_around_fsm.h" |
6 |
#include "lemmings.h" |
7 |
|
8 |
/*A simple behavior for following the leader.
|
9 |
|
10 |
SINGLE_FILE pattern not implemented yet
|
11 |
*/
|
12 |
|
13 |
|
14 |
void lemmings_init()
|
15 |
{ |
16 |
run_around_init(); // prepare for moving
|
17 |
analog_init(); |
18 |
motors_init(); |
19 |
wl_init(); |
20 |
wl_token_ring_register(); |
21 |
wl_token_ring_join(); // join token ring
|
22 |
|
23 |
/*Start in the default state, LEAD*/
|
24 |
cur_state = LEAD; |
25 |
/*Set timers to their maximum values.*/
|
26 |
crazy_count=CRAZY_MAX; |
27 |
|
28 |
// set to default state
|
29 |
follow_bot = -1;
|
30 |
cur_state = LEAD; |
31 |
|
32 |
// set pattern
|
33 |
lemmings_set_pattern(FOLLOW_MULTI_DEFAULT); |
34 |
} |
35 |
|
36 |
// set the following pattern
|
37 |
// SINGLE_FILE if pattern <= 0
|
38 |
// TREE if pattern >= 1
|
39 |
void lemmings_set_pattern(int pattern) |
40 |
{ |
41 |
if (pattern <= 0) |
42 |
follow_multi = 0;
|
43 |
else
|
44 |
follow_multi = 1;
|
45 |
} |
46 |
|
47 |
/*The main function, call this to update states as frequently as possible.*/
|
48 |
void lemmings_FSM(void) { |
49 |
int bom=0; |
50 |
|
51 |
if (follow_bot < 0) |
52 |
lemmings_Prims(); // if we are not following, then attempt to follow
|
53 |
|
54 |
usb_puts("following: ");
|
55 |
usb_puti(follow_bot); |
56 |
|
57 |
// if we are following a bot, make sure we can still see it
|
58 |
while (follow_bot >= 0) { |
59 |
bom = wl_token_get_sensor_reading(wl_get_xbee_id(),follow_bot); |
60 |
if (bom < 0) { |
61 |
// get new bot to follow
|
62 |
lemmings_Prims(); |
63 |
continue;
|
64 |
} |
65 |
else {
|
66 |
cur_state = FOLLOW; // set to follow state
|
67 |
switch(bom) {
|
68 |
case 0: |
69 |
case 15: |
70 |
case 14: |
71 |
case 13: |
72 |
case 12: |
73 |
follow_direction = 255;
|
74 |
break;
|
75 |
case 1: |
76 |
follow_direction = 150;
|
77 |
break;
|
78 |
case 2: |
79 |
follow_direction = 100;
|
80 |
break;
|
81 |
case 3: |
82 |
follow_direction = 50;
|
83 |
break;
|
84 |
case 4: |
85 |
follow_direction = 0;
|
86 |
break;
|
87 |
case 5: |
88 |
follow_direction = -50;
|
89 |
break;
|
90 |
case 6: |
91 |
follow_direction = -100;
|
92 |
break;
|
93 |
case 7: |
94 |
follow_direction = -150;
|
95 |
break;
|
96 |
case 8: |
97 |
case 9: |
98 |
case 10: |
99 |
case 11: |
100 |
follow_direction = -255;
|
101 |
break;
|
102 |
default:
|
103 |
follow_direction = 0;
|
104 |
break;
|
105 |
} |
106 |
break; // break from loop |
107 |
} |
108 |
} |
109 |
|
110 |
|
111 |
/*If the crazy count is in it's >>3 range, and we are currently a follower, be a leader.*/
|
112 |
if(cur_state == FOLLOW && crazy_count<=(CRAZY_MAX>>3)) |
113 |
{ |
114 |
cur_state = LEAD; |
115 |
} |
116 |
|
117 |
evaluate_state(); // evaluate state
|
118 |
} |
119 |
|
120 |
// create connected components and pick a leader for each chain
|
121 |
// use modified Prim's alogrithm to find local spanning tree
|
122 |
// CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW
|
123 |
void lemmings_Prims(void) { |
124 |
|
125 |
int nodeA = wl_get_xbee_id(); // get id of current robot |
126 |
int nodeB = -1; |
127 |
int cur_weight = -1; |
128 |
int best_weight = 1000; // set to infinity initially |
129 |
int best_node = -1; |
130 |
|
131 |
// iterate through token ring
|
132 |
wl_token_iterator_begin(); |
133 |
while(wl_token_iterator_has_next()) {
|
134 |
nodeB = wl_token_iterator_next(); |
135 |
if (nodeB < 0) |
136 |
break; // no more bots |
137 |
else if (nodeB == nodeA) |
138 |
continue; // can't follow self |
139 |
|
140 |
cur_weight = lemmings_get_edge_weight(nodeA,nodeB); |
141 |
if (cur_weight < best_weight) {
|
142 |
// this is new best node, so save values
|
143 |
best_weight = cur_weight; |
144 |
best_node = nodeB; |
145 |
} |
146 |
} |
147 |
|
148 |
if (best_node == -1 || (best_weight > 5 && nodeA < nodeB)) { |
149 |
// no robots to follow, or choosing to be a leader
|
150 |
cur_state = LEAD; |
151 |
follow_bot = -1;
|
152 |
} |
153 |
else {
|
154 |
// follow bot
|
155 |
cur_state = FOLLOW; |
156 |
follow_bot = nodeB; |
157 |
} |
158 |
} |
159 |
|
160 |
// get edge weight of sensor matrix
|
161 |
// add in BOM range data when BOM 1.5 comes out
|
162 |
int lemmings_get_edge_weight(int robot1, int robot2) { |
163 |
int weight = 0; |
164 |
int bom = wl_token_get_sensor_reading(robot1,robot2);
|
165 |
// since we only have positioning data, give better weight to
|
166 |
// bots in from of us
|
167 |
switch(bom) {
|
168 |
case 12: |
169 |
weight++; |
170 |
case 13: |
171 |
case 11: |
172 |
weight++; |
173 |
case 14: |
174 |
case 10: |
175 |
weight++; |
176 |
case 15: |
177 |
case 9: |
178 |
weight++; |
179 |
case 0: |
180 |
case 8: |
181 |
weight++; |
182 |
case 1: |
183 |
case 7: |
184 |
weight++; |
185 |
case 2: |
186 |
case 6: |
187 |
weight++; |
188 |
case 3: |
189 |
case 5: |
190 |
weight++; |
191 |
case 4: |
192 |
break;
|
193 |
default:
|
194 |
weight = -1;
|
195 |
break;
|
196 |
} |
197 |
return weight;
|
198 |
} |
199 |
|
200 |
|
201 |
//Acts on state change.
|
202 |
void lemmings_evaluate_state(){
|
203 |
switch(cur_state){
|
204 |
case(LEAD):
|
205 |
run_around_FSM(); // do random run around
|
206 |
break;
|
207 |
|
208 |
case(FOLLOW):
|
209 |
move_avoid(200,follow_direction,33); // move in direction of bot |
210 |
break;
|
211 |
|
212 |
default:
|
213 |
/*Should never get here, go strait.*/
|
214 |
move(200,0); |
215 |
break;
|
216 |
} |
217 |
} |
218 |
|
219 |
|