root / trunk / code / behaviors / lemmings / lemmings.c @ 1514
History | View | Annotate | Download (7.64 KB)
1 | 700 | dsschult | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wl_defs.h> |
||
3 | #include <wireless.h> |
||
4 | #include <wl_token_ring.h> |
||
5 | //#include <colonet_dragonfly.h>
|
||
6 | //#include "smart_run_around_fsm.h"
|
||
7 | #include "lemmings.h" |
||
8 | |||
9 | /*A simple behavior for following the leader
|
||
10 | SINGLE_FILE pattern not implemented yet
|
||
11 | */
|
||
12 | |||
13 | // FSM states
|
||
14 | #define LEAD 0 // be a leader |
||
15 | #define FOLLOW 1 // follow another robot |
||
16 | |||
17 | #define FOLLOW_MULTI_DEFAULT 1 // set default multi-following pattern |
||
18 | #define SPEED 200 // set default speed |
||
19 | |||
20 | #define LEMMINGS_PAN 7 // set the lemmings PAN |
||
21 | |||
22 | /* Globals */
|
||
23 | static int cur_state; // current state |
||
24 | static int local_leader; // follow this bot (-1 if leader) |
||
25 | static int global_leader; // this is the global leader |
||
26 | static int follow_lock; // this locks in the bot to follow |
||
27 | static int follow_multi; // set to 0 for single-file following, 1 for tree following |
||
28 | static int local_id; // this is the local robot id |
||
29 | static int speed; // robot's forward speed |
||
30 | static int turn_speed; // robot's turning speed |
||
31 | static int pan; // set the lemmings PAN |
||
32 | |||
33 | /* Internal prototypes */
|
||
34 | static int get_local_leader(void); |
||
35 | static int get_edge_weight(int robot1, int robot2); |
||
36 | static void get_bom_velocity(int bom_id); |
||
37 | |||
38 | void lemmings_init() {
|
||
39 | // usb_puts("lemmings_init\n");
|
||
40 | |||
41 | // colonet_init();
|
||
42 | //run_around_init(); // prepare for moving
|
||
43 | //range_init();
|
||
44 | //analog_init();
|
||
45 | /* range finder code interferes with token ring */
|
||
46 | orb_init(); |
||
47 | orb_enable(); |
||
48 | |||
49 | // set to defaults
|
||
50 | cur_state = FOLLOW; |
||
51 | local_leader = -1;
|
||
52 | global_leader = -1;
|
||
53 | follow_lock = 0;
|
||
54 | local_id = wl_get_xbee_id(); |
||
55 | speed = SPEED; |
||
56 | turn_speed = 0;
|
||
57 | pan = LEMMINGS_PAN; |
||
58 | |||
59 | follow_multi = FOLLOW_MULTI_DEFAULT; |
||
60 | } |
||
61 | |||
62 | /* The main function, call this to update states as frequently as possible. */
|
||
63 | void lemmings_FSM(void) { |
||
64 | int leader_bom = 0; |
||
65 | static int leader_check_count = 0; |
||
66 | |||
67 | |||
68 | if (wl_token_get_num_robots() <= 1) { |
||
69 | /* if there is only one robot in the token ring, there is nothing to
|
||
70 | follow, so don't do anything */
|
||
71 | motors_off(); |
||
72 | speed = 0;
|
||
73 | orb_set_color(GREEN); |
||
74 | return;
|
||
75 | } |
||
76 | |||
77 | //usb_puts("state: ");
|
||
78 | //usb_puti(cur_state);
|
||
79 | //usb_puts("\n");
|
||
80 | |||
81 | // only check for new global leader once every 5 iterations
|
||
82 | leader_check_count++; |
||
83 | if (leader_check_count % 3 == 0) { |
||
84 | |||
85 | if (leader_check_count % 12 == 0) { |
||
86 | // find robot with max id and assign as leader
|
||
87 | int max = -1; |
||
88 | wl_token_iterator_begin(); |
||
89 | while (wl_token_iterator_has_next()) {
|
||
90 | int n = wl_token_iterator_next();
|
||
91 | if (n > max) {
|
||
92 | max = n; |
||
93 | } |
||
94 | /* usb_puti(n); */
|
||
95 | /* usb_puts("\n"); */
|
||
96 | } |
||
97 | |||
98 | if (max == local_id) {
|
||
99 | // become the global leader
|
||
100 | orb_set_color(BLUE); |
||
101 | cur_state = LEAD; |
||
102 | local_leader = -1;
|
||
103 | follow_lock = 0;
|
||
104 | speed = SPEED; |
||
105 | turn_speed = 0;
|
||
106 | // usb_puts(" becoming leader \n");
|
||
107 | } |
||
108 | else {
|
||
109 | // become a follower
|
||
110 | orb_set_color(RED); |
||
111 | cur_state = FOLLOW; |
||
112 | // usb_puts(" becoming follower\n");
|
||
113 | } |
||
114 | |||
115 | global_leader = max; // set the global leader
|
||
116 | } |
||
117 | |||
118 | if (cur_state == FOLLOW) {
|
||
119 | // Make sure we can see our local leader
|
||
120 | leader_bom = (local_leader>=0)?wl_token_get_sensor_reading(local_id, local_leader):-1; |
||
121 | |||
122 | if (leader_bom < 0) { |
||
123 | if (follow_lock) {
|
||
124 | // unlock bot to follow
|
||
125 | follow_lock = 0;
|
||
126 | // actually change to follow other bot on next loop
|
||
127 | } |
||
128 | else {
|
||
129 | //usb_puts("no connection to leader\n");
|
||
130 | |||
131 | // Can't see our old leader -- get new bot to follow.
|
||
132 | local_leader = get_local_leader(); |
||
133 | |||
134 | if (local_leader == -1) { |
||
135 | /* if there is only one robot in the token ring, there is nothing to
|
||
136 | follow, so don't do anything */
|
||
137 | /*motors_off();
|
||
138 | speed = 0;
|
||
139 | return;*/
|
||
140 | // nothing to follow, so become a local_leader and start leading
|
||
141 | //usb_puts("can't see anyone - becoming a leader\n");
|
||
142 | cur_state = LEAD; |
||
143 | local_leader = -1;
|
||
144 | orb_set_color(BLUE); |
||
145 | speed = SPEED; |
||
146 | turn_speed = 0;
|
||
147 | } else {
|
||
148 | //usb_puts("new leader is");
|
||
149 | //usb_puti(local_leader);
|
||
150 | //usb_puts("\n");
|
||
151 | // get data in order to follow leader
|
||
152 | leader_bom = wl_token_get_sensor_reading(local_id, local_leader); |
||
153 | get_bom_velocity(leader_bom); |
||
154 | } |
||
155 | } |
||
156 | } else {
|
||
157 | // Leader is still in sight, so follow it
|
||
158 | get_bom_velocity(leader_bom); |
||
159 | follow_lock = 1; // lock in bot to follow |
||
160 | } |
||
161 | } |
||
162 | } |
||
163 | else {
|
||
164 | // prevent spinning in circles
|
||
165 | turn_speed /= 5;
|
||
166 | } |
||
167 | |||
168 | // check if running into something
|
||
169 | /*int range = range_read_distance(IR2);
|
||
170 | if (range > 0 && range < 150) {
|
||
171 | if (cur_state == LEAD) {
|
||
172 | turn_speed = 100; // turn to avoid
|
||
173 | }
|
||
174 | else {
|
||
175 | speed = 0; // stop to avoid
|
||
176 | }
|
||
177 | }*/
|
||
178 | |||
179 | // do movement
|
||
180 | move(speed, turn_speed); |
||
181 | } |
||
182 | |||
183 | // translate bom reading into direction and speed
|
||
184 | static void get_bom_velocity(int bom_id) { |
||
185 | int error = 0; |
||
186 | |||
187 | if (bom_id < 0 || bom_id > 15) { |
||
188 | // usb_puts("ERROR - invalid bom_id\n");
|
||
189 | turn_speed = 0;
|
||
190 | speed = 0;
|
||
191 | return;
|
||
192 | } |
||
193 | |||
194 | // set so right is negative, left is positive
|
||
195 | if (bom_id <= 12) { |
||
196 | error = bom_id - 4;
|
||
197 | } else {
|
||
198 | error = bom_id - 20;
|
||
199 | } |
||
200 | |||
201 | // set turn speed and main speed
|
||
202 | // if error is small, turn slowly and travel at normal speed
|
||
203 | // if error is large, turn faster and travel at reduced speed
|
||
204 | switch(error)
|
||
205 | { |
||
206 | case 0: |
||
207 | turn_speed = 0;
|
||
208 | speed = SPEED; |
||
209 | return;
|
||
210 | case 1: |
||
211 | case -1: |
||
212 | turn_speed = error * 5;
|
||
213 | speed = SPEED; |
||
214 | return;
|
||
215 | case 2: |
||
216 | case -2: |
||
217 | turn_speed = error * 8;
|
||
218 | speed = SPEED; |
||
219 | return;
|
||
220 | case 3: |
||
221 | case -3: |
||
222 | turn_speed = error * 12;
|
||
223 | speed = SPEED; |
||
224 | return;
|
||
225 | case 4: |
||
226 | case -4: |
||
227 | turn_speed = error * 16;
|
||
228 | speed = SPEED - SPEED * error * error / 36;
|
||
229 | return;
|
||
230 | } |
||
231 | // if not in a case
|
||
232 | turn_speed = error * 20;
|
||
233 | speed = SPEED - SPEED * error * error / 36;
|
||
234 | } |
||
235 | |||
236 | // create connected components and pick a leader for each chain
|
||
237 | // CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW
|
||
238 | static int get_local_leader(void) { |
||
239 | int nodeB;
|
||
240 | int cur_weight;
|
||
241 | int best_weight = 1000; // set to infinity initially |
||
242 | int best_node = -1; |
||
243 | |||
244 | // check for global leader first
|
||
245 | int test = wl_token_get_sensor_reading(local_id, global_leader);
|
||
246 | if (test != -1) |
||
247 | return global_leader;
|
||
248 | |||
249 | wl_token_iterator_begin(); |
||
250 | |||
251 | // iterator through robots in token ring
|
||
252 | while (wl_token_iterator_has_next()) {
|
||
253 | nodeB = wl_token_iterator_next(); |
||
254 | if (nodeB == local_id) {
|
||
255 | continue; // can't follow self |
||
256 | } |
||
257 | |||
258 | // get an edge weight based on the robot position
|
||
259 | cur_weight = get_edge_weight(local_id, nodeB); |
||
260 | |||
261 | if (nodeB == global_leader && cur_weight != -1) { |
||
262 | // always follow the global leader if you have the option to
|
||
263 | best_node = nodeB; |
||
264 | break;
|
||
265 | } |
||
266 | |||
267 | if (cur_weight >= 0 && cur_weight < best_weight) { |
||
268 | // this is new best node, so save values.
|
||
269 | best_weight = cur_weight; |
||
270 | best_node = nodeB; |
||
271 | } |
||
272 | } |
||
273 | |||
274 | return best_node;
|
||
275 | } |
||
276 | |||
277 | // get edge weight of sensor matrix
|
||
278 | // add in BOM range data when BOM 1.5 comes out
|
||
279 | static int get_edge_weight(int robot1, int robot2) { |
||
280 | int bom = wl_token_get_sensor_reading(robot1,robot2);
|
||
281 | |||
282 | // Robots closer to directly in front of us have lower weight.
|
||
283 | switch(bom) {
|
||
284 | case 12: |
||
285 | return 10; |
||
286 | |||
287 | case 13: |
||
288 | case 11: |
||
289 | return 9; |
||
290 | |||
291 | case 14: |
||
292 | case 10: |
||
293 | return 8; |
||
294 | |||
295 | case 15: |
||
296 | case 9: |
||
297 | return 7; |
||
298 | |||
299 | case 0: |
||
300 | case 8: |
||
301 | return 6; |
||
302 | |||
303 | case 1: |
||
304 | case 7: |
||
305 | return 5; |
||
306 | |||
307 | case 2: |
||
308 | case 6: |
||
309 | return 4; |
||
310 | |||
311 | case 3: |
||
312 | case 5: |
||
313 | return 3; |
||
314 | |||
315 | case 4: |
||
316 | return 2; |
||
317 | } |
||
318 | |||
319 | return -1; |
||
320 | } |