root / branches / lemmings / code / behaviors / lemmings / lemmings.c @ 332
History | View | Annotate | Download (7.64 KB)
1 | 209 | dsschult | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wl_defs.h> |
||
3 | #include <wireless.h> |
||
4 | #include <wl_token_ring.h> |
||
5 | 243 | emarinel | //#include <colonet_dragonfly.h>
|
6 | 258 | dsschult | //#include "smart_run_around_fsm.h"
|
7 | 209 | dsschult | #include "lemmings.h" |
8 | |||
9 | 258 | dsschult | /*A simple behavior for following the leader
|
10 | 209 | dsschult | SINGLE_FILE pattern not implemented yet
|
11 | */
|
||
12 | |||
13 | 233 | emarinel | // FSM states
|
14 | 258 | dsschult | #define LEAD 0 // be a leader |
15 | #define FOLLOW 1 // follow another robot |
||
16 | 209 | dsschult | |
17 | 230 | emarinel | #define FOLLOW_MULTI_DEFAULT 1 // set default multi-following pattern |
18 | 258 | dsschult | #define SPEED 200 // set default speed |
19 | 230 | emarinel | |
20 | 305 | dsschult | #define LEMMINGS_PAN 7 // set the lemmings PAN |
21 | |||
22 | 230 | emarinel | /* Globals */
|
23 | 239 | emarinel | static int cur_state; // current state |
24 | 258 | dsschult | static int local_leader; // follow this bot (-1 if leader) |
25 | static int global_leader; // this is the global leader |
||
26 | 305 | dsschult | static int follow_lock; // this locks in the bot to follow |
27 | 258 | dsschult | 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 | 295 | dsschult | static int speed; // robot's forward speed |
30 | static int turn_speed; // robot's turning speed |
||
31 | 305 | dsschult | static int pan; // set the lemmings PAN |
32 | 230 | emarinel | |
33 | 233 | emarinel | /* Internal prototypes */
|
34 | static int get_local_leader(void); |
||
35 | static int get_edge_weight(int robot1, int robot2); |
||
36 | 295 | dsschult | static void get_bom_velocity(int bom_id); |
37 | 230 | emarinel | |
38 | void lemmings_init() {
|
||
39 | 239 | emarinel | // usb_puts("lemmings_init\n");
|
40 | 230 | emarinel | |
41 | 258 | dsschult | // colonet_init();
|
42 | //run_around_init(); // prepare for moving
|
||
43 | 295 | dsschult | //range_init();
|
44 | //analog_init();
|
||
45 | /* range finder code interferes with token ring */
|
||
46 | orb_init(); |
||
47 | orb_enable(); |
||
48 | 230 | emarinel | |
49 | 258 | dsschult | // set to defaults
|
50 | 243 | emarinel | cur_state = FOLLOW; |
51 | 233 | emarinel | local_leader = -1;
|
52 | 243 | emarinel | global_leader = -1;
|
53 | 305 | dsschult | follow_lock = 0;
|
54 | 233 | emarinel | local_id = wl_get_xbee_id(); |
55 | 295 | dsschult | speed = SPEED; |
56 | turn_speed = 0;
|
||
57 | 305 | dsschult | pan = LEMMINGS_PAN; |
58 | 230 | emarinel | |
59 | 233 | emarinel | follow_multi = FOLLOW_MULTI_DEFAULT; |
60 | } |
||
61 | 230 | emarinel | |
62 | 233 | emarinel | /* The main function, call this to update states as frequently as possible. */
|
63 | void lemmings_FSM(void) { |
||
64 | 295 | dsschult | int leader_bom = 0; |
65 | 243 | emarinel | static int leader_check_count = 0; |
66 | 230 | emarinel | |
67 | 243 | emarinel | |
68 | if (wl_token_get_num_robots() <= 1) { |
||
69 | 258 | dsschult | /* if there is only one robot in the token ring, there is nothing to
|
70 | follow, so don't do anything */
|
||
71 | 243 | emarinel | motors_off(); |
72 | 295 | dsschult | speed = 0;
|
73 | orb_set_color(GREEN); |
||
74 | 243 | emarinel | return;
|
75 | } |
||
76 | |||
77 | 239 | emarinel | //usb_puts("state: ");
|
78 | //usb_puti(cur_state);
|
||
79 | //usb_puts("\n");
|
||
80 | 209 | dsschult | |
81 | 258 | dsschult | // only check for new global leader once every 5 iterations
|
82 | 243 | emarinel | leader_check_count++; |
83 | 295 | dsschult | if (leader_check_count % 3 == 0) { |
84 | 243 | emarinel | |
85 | 305 | dsschult | if (leader_check_count % 12 == 0) { |
86 | 295 | dsschult | // 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 | 239 | emarinel | } |
97 | 209 | dsschult | |
98 | 295 | dsschult | if (max == local_id) {
|
99 | // become the global leader
|
||
100 | orb_set_color(BLUE); |
||
101 | cur_state = LEAD; |
||
102 | local_leader = -1;
|
||
103 | 305 | dsschult | follow_lock = 0;
|
104 | 295 | dsschult | 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 | 243 | emarinel | // usb_puts(" becoming follower\n");
|
113 | 295 | dsschult | } |
114 | |||
115 | global_leader = max; // set the global leader
|
||
116 | 233 | emarinel | } |
117 | 295 | dsschult | |
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 | 230 | emarinel | |
122 | 295 | dsschult | if (leader_bom < 0) { |
123 | 305 | dsschult | 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 | 239 | emarinel | |
131 | 305 | dsschult | // 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 | 295 | dsschult | } |
156 | 239 | emarinel | } else {
|
157 | 295 | dsschult | // Leader is still in sight, so follow it
|
158 | get_bom_velocity(leader_bom); |
||
159 | 305 | dsschult | follow_lock = 1; // lock in bot to follow |
160 | 209 | dsschult | } |
161 | } |
||
162 | 258 | dsschult | } |
163 | 295 | dsschult | 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 | 233 | emarinel | } |
182 | 230 | emarinel | |
183 | 258 | dsschult | // translate bom reading into direction and speed
|
184 | 295 | dsschult | static void get_bom_velocity(int bom_id) { |
185 | 239 | emarinel | int error = 0; |
186 | 233 | emarinel | |
187 | 239 | emarinel | if (bom_id < 0 || bom_id > 15) { |
188 | // usb_puts("ERROR - invalid bom_id\n");
|
||
189 | 295 | dsschult | turn_speed = 0;
|
190 | speed = 0;
|
||
191 | 243 | emarinel | return;
|
192 | 239 | emarinel | } |
193 | 233 | emarinel | |
194 | 258 | dsschult | // set so right is negative, left is positive
|
195 | 239 | emarinel | if (bom_id <= 12) { |
196 | error = bom_id - 4;
|
||
197 | } else {
|
||
198 | error = bom_id - 20;
|
||
199 | 233 | emarinel | } |
200 | |||
201 | 271 | dsschult | // 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 | 295 | dsschult | turn_speed = 0;
|
208 | speed = SPEED; |
||
209 | 271 | dsschult | return;
|
210 | case 1: |
||
211 | case -1: |
||
212 | 295 | dsschult | turn_speed = error * 5;
|
213 | speed = SPEED; |
||
214 | 271 | dsschult | return;
|
215 | case 2: |
||
216 | case -2: |
||
217 | 295 | dsschult | turn_speed = error * 8;
|
218 | speed = SPEED; |
||
219 | 271 | dsschult | return;
|
220 | case 3: |
||
221 | case -3: |
||
222 | 295 | dsschult | turn_speed = error * 12;
|
223 | speed = SPEED; |
||
224 | 271 | dsschult | return;
|
225 | case 4: |
||
226 | case -4: |
||
227 | 295 | dsschult | turn_speed = error * 16;
|
228 | speed = SPEED - SPEED * error * error / 36;
|
||
229 | 271 | dsschult | return;
|
230 | } |
||
231 | // if not in a case
|
||
232 | 295 | dsschult | turn_speed = error * 20;
|
233 | speed = SPEED - SPEED * error * error / 36;
|
||
234 | 209 | dsschult | } |
235 | |||
236 | // create connected components and pick a leader for each chain
|
||
237 | // CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW
|
||
238 | 233 | emarinel | static int get_local_leader(void) { |
239 | int nodeB;
|
||
240 | int cur_weight;
|
||
241 | 209 | dsschult | int best_weight = 1000; // set to infinity initially |
242 | int best_node = -1; |
||
243 | 305 | dsschult | |
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 | 230 | emarinel | |
249 | 209 | dsschult | wl_token_iterator_begin(); |
250 | 230 | emarinel | |
251 | 258 | dsschult | // iterator through robots in token ring
|
252 | 233 | emarinel | while (wl_token_iterator_has_next()) {
|
253 | 209 | dsschult | nodeB = wl_token_iterator_next(); |
254 | 233 | emarinel | if (nodeB == local_id) {
|
255 | 209 | dsschult | continue; // can't follow self |
256 | 230 | emarinel | } |
257 | |||
258 | 258 | dsschult | // get an edge weight based on the robot position
|
259 | 233 | emarinel | cur_weight = get_edge_weight(local_id, nodeB); |
260 | 230 | emarinel | |
261 | 243 | emarinel | if (nodeB == global_leader && cur_weight != -1) { |
262 | 258 | dsschult | // always follow the global leader if you have the option to
|
263 | 243 | emarinel | best_node = nodeB; |
264 | break;
|
||
265 | } |
||
266 | |||
267 | 212 | dsschult | if (cur_weight >= 0 && cur_weight < best_weight) { |
268 | 233 | emarinel | // this is new best node, so save values.
|
269 | 209 | dsschult | best_weight = cur_weight; |
270 | best_node = nodeB; |
||
271 | } |
||
272 | } |
||
273 | 230 | emarinel | |
274 | 233 | emarinel | return best_node;
|
275 | 209 | dsschult | } |
276 | |||
277 | // get edge weight of sensor matrix
|
||
278 | // add in BOM range data when BOM 1.5 comes out
|
||
279 | 233 | emarinel | static int get_edge_weight(int robot1, int robot2) { |
280 | 209 | dsschult | int bom = wl_token_get_sensor_reading(robot1,robot2);
|
281 | 233 | emarinel | |
282 | // Robots closer to directly in front of us have lower weight.
|
||
283 | 209 | dsschult | switch(bom) {
|
284 | case 12: |
||
285 | 233 | emarinel | return 10; |
286 | |||
287 | 209 | dsschult | case 13: |
288 | case 11: |
||
289 | 233 | emarinel | return 9; |
290 | |||
291 | 209 | dsschult | case 14: |
292 | case 10: |
||
293 | 233 | emarinel | return 8; |
294 | |||
295 | 209 | dsschult | case 15: |
296 | case 9: |
||
297 | 233 | emarinel | return 7; |
298 | |||
299 | 209 | dsschult | case 0: |
300 | case 8: |
||
301 | 233 | emarinel | return 6; |
302 | |||
303 | 209 | dsschult | case 1: |
304 | case 7: |
||
305 | 233 | emarinel | return 5; |
306 | |||
307 | 209 | dsschult | case 2: |
308 | case 6: |
||
309 | 233 | emarinel | return 4; |
310 | |||
311 | 209 | dsschult | case 3: |
312 | case 5: |
||
313 | 233 | emarinel | return 3; |
314 | |||
315 | 209 | dsschult | case 4: |
316 | 233 | emarinel | return 2; |
317 | 209 | dsschult | } |
318 | |||
319 | 233 | emarinel | return -1; |
320 | 209 | dsschult | } |