root / demos / smart_run_around_fsm / lib / src / libwireless / wl_token_ring.c @ 1784
History | View | Annotate | Download (23.2 KB)
1 | 242 | bcoltin | /**
|
---|---|---|---|
2 | * Copyright (c) 2007 Colony Project
|
||
3 | 397 | emarinel | *
|
4 | 242 | bcoltin | * Permission is hereby granted, free of charge, to any person
|
5 | * obtaining a copy of this software and associated documentation
|
||
6 | * files (the "Software"), to deal in the Software without
|
||
7 | * restriction, including without limitation the rights to use,
|
||
8 | * copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
9 | * copies of the Software, and to permit persons to whom the
|
||
10 | * Software is furnished to do so, subject to the following
|
||
11 | * conditions:
|
||
12 | 397 | emarinel | *
|
13 | 242 | bcoltin | * The above copyright notice and this permission notice shall be
|
14 | * included in all copies or substantial portions of the Software.
|
||
15 | 397 | emarinel | *
|
16 | 242 | bcoltin | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
17 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
|
||
18 | * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
||
19 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||
20 | * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
|
||
21 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
||
22 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||
23 | * OTHER DEALINGS IN THE SOFTWARE.
|
||
24 | **/
|
||
25 | |||
26 | /**
|
||
27 | * @file wl_token_ring.c
|
||
28 | * @brief Token Ring Implementation
|
||
29 | *
|
||
30 | * Implementation of the token ring packet group.
|
||
31 | *
|
||
32 | * @author Brian Coltin, Colony Project, CMU Robotics Club
|
||
33 | **/
|
||
34 | |||
35 | 17 | bcoltin | #include <wl_token_ring.h> |
36 | |||
37 | #include <stdlib.h> |
||
38 | |||
39 | #include <wl_defs.h> |
||
40 | #include <wireless.h> |
||
41 | #include <sensor_matrix.h> |
||
42 | |||
43 | #ifdef ROBOT
|
||
44 | 86 | bcoltin | #ifndef FIREFLY
|
45 | 17 | bcoltin | #include <bom.h> |
46 | 86 | bcoltin | #endif
|
47 | 17 | bcoltin | #include <time.h> |
48 | #endif
|
||
49 | |||
50 | |||
51 | 1460 | dsschult | //#define DEFAULT_SENSOR_MATRIX_SIZE 20
|
52 | |||
53 | 17 | bcoltin | /*Ring States*/
|
54 | |||
55 | #define NONMEMBER 0 |
||
56 | #define MEMBER 1 |
||
57 | #define JOINING 2 |
||
58 | #define ACCEPTED 3 |
||
59 | #define LEAVING 4 |
||
60 | |||
61 | /*Frame Types*/
|
||
62 | 736 | bcoltin | #define TOKEN_JOIN_ACCEPT_FRAME 1 |
63 | #define WL_TOKEN_PASS_FRAME 2 |
||
64 | 17 | bcoltin | |
65 | /*Function Prototypes*/
|
||
66 | |||
67 | /*Wireless Library Prototypes*/
|
||
68 | 418 | emarinel | static void wl_token_ring_timeout_handler(void); |
69 | static void wl_token_ring_response_handler(int frame, int received); |
||
70 | static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length); |
||
71 | static void wl_token_ring_cleanup(void); |
||
72 | 17 | bcoltin | |
73 | /*Helper Functions*/
|
||
74 | 423 | emarinel | static int wl_token_pass_token(void); |
75 | 418 | emarinel | static int get_token_distance(int robot1, int robot2); |
76 | static void wl_token_get_token(void); |
||
77 | 17 | bcoltin | |
78 | /*Packet Handling Routines*/
|
||
79 | 736 | bcoltin | static void wl_token_pass_receive(int source); |
80 | static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength); |
||
81 | 418 | emarinel | static void wl_token_bom_on_receive(int source); |
82 | static void wl_token_join_receive(int source); |
||
83 | static void wl_token_join_accept_receive(int source); |
||
84 | 17 | bcoltin | |
85 | /*Global Variables*/
|
||
86 | |||
87 | //the robot we are waiting to say it has received the token. -1 if unspecified
|
||
88 | 418 | emarinel | static int wl_token_next_robot = -1; |
89 | 17 | bcoltin | |
90 | //true if the robot should be in the token ring, 0 otherwise
|
||
91 | 418 | emarinel | static int ringState = NONMEMBER; |
92 | 17 | bcoltin | //the id of the robot who accepted us into the token ring, only used in ACCEPTED state
|
93 | 418 | emarinel | static int acceptor = -1; |
94 | 17 | bcoltin | //id of the robot we are accepting
|
95 | 418 | emarinel | static int accepted = -1; |
96 | 17 | bcoltin | |
97 | //the counter for when we assume a robot is dead
|
||
98 | 418 | emarinel | static int deathDelay = -1; |
99 | 17 | bcoltin | //the counter for joining, before we form our own token ring
|
100 | 418 | emarinel | static int joinDelay = -1; |
101 | 17 | bcoltin | |
102 | 52 | bcoltin | //current robot to check in the iterator
|
103 | 418 | emarinel | static int iteratorCount = 0; |
104 | 52 | bcoltin | |
105 | 196 | bcoltin | // the amount of time a robot has had its BOM on for
|
106 | 418 | emarinel | static int bom_on_count = 0; |
107 | 196 | bcoltin | |
108 | 736 | bcoltin | #ifndef ROBOT
|
109 | 418 | emarinel | static void do_nothing(void) {} |
110 | static int get_nothing(void) {return -1;} |
||
111 | 736 | bcoltin | #endif
|
112 | 86 | bcoltin | |
113 | 17 | bcoltin | #ifdef ROBOT
|
114 | 86 | bcoltin | #ifndef FIREFLY
|
115 | 418 | emarinel | static void (*bom_on_function) (void) = bom_on; |
116 | static void (*bom_off_function) (void) = bom_off; |
||
117 | static int (*get_max_bom_function) (void) = get_max_bom; |
||
118 | 17 | bcoltin | #else
|
119 | 418 | emarinel | static void (*bom_on_function) (void) = do_nothing; |
120 | static void (*bom_off_function) (void) = do_nothing; |
||
121 | static int (*get_max_bom_function) (void) = get_nothing; |
||
122 | 17 | bcoltin | #endif
|
123 | 86 | bcoltin | #else
|
124 | 418 | emarinel | static void (*bom_on_function) (void) = do_nothing; |
125 | static void (*bom_off_function) (void) = do_nothing; |
||
126 | static int (*get_max_bom_function) (void) = get_nothing; |
||
127 | 86 | bcoltin | #endif
|
128 | 17 | bcoltin | |
129 | 418 | emarinel | static PacketGroupHandler wl_token_ring_handler =
|
130 | {WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler, |
||
131 | 346 | bcoltin | wl_token_ring_response_handler, wl_token_ring_receive_handler, |
132 | wl_token_ring_cleanup}; |
||
133 | 17 | bcoltin | |
134 | /**
|
||
135 | 423 | emarinel | * Causes the robot to join an existing token ring, or create one
|
136 | * if no token ring exists. The token ring uses global and robot to robot
|
||
137 | * packets, and does not rely on any PAN.
|
||
138 | **/
|
||
139 | int wl_token_ring_join()
|
||
140 | { |
||
141 | WL_DEBUG_PRINT("Joining the token ring.\r\n");
|
||
142 | |||
143 | ringState = JOINING; |
||
144 | joinDelay = DEATH_DELAY * 2;
|
||
145 | if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN, NULL, 0, 0) != 0) { |
||
146 | return -1; |
||
147 | } |
||
148 | |||
149 | return 0; |
||
150 | } |
||
151 | |||
152 | /**
|
||
153 | * Causes the robot to leave the token ring. The robot stops
|
||
154 | * alerting others of its location, but continues storing the
|
||
155 | * locations of other robots.
|
||
156 | **/
|
||
157 | void wl_token_ring_leave()
|
||
158 | { |
||
159 | ringState = LEAVING; |
||
160 | } |
||
161 | |||
162 | /**
|
||
163 | 17 | bcoltin | * Initialize the token ring packet group and register it with the
|
164 | * wireless library. The robot will not join a token ring.
|
||
165 | **/
|
||
166 | 418 | emarinel | int wl_token_ring_register()
|
167 | 346 | bcoltin | { |
168 | if (wl_get_xbee_id() > 0xFF) |
||
169 | { |
||
170 | //Note: if this becomes an issue (unlikely), we could limit sensor information
|
||
171 | //to half a byte and use 12 bits for the id
|
||
172 | WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");
|
||
173 | WL_DEBUG_PRINT_INT(wl_get_xbee_id()); |
||
174 | WL_DEBUG_PRINT(".\r\n");
|
||
175 | 418 | emarinel | return -1; |
176 | 346 | bcoltin | } |
177 | 397 | emarinel | |
178 | 736 | bcoltin | sensor_matrix_create(); |
179 | 346 | bcoltin | //add ourselves to the sensor matrix
|
180 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
|
181 | 17 | bcoltin | |
182 | 346 | bcoltin | wl_register_packet_group(&wl_token_ring_handler); |
183 | 418 | emarinel | |
184 | return 0; |
||
185 | 17 | bcoltin | } |
186 | |||
187 | /**
|
||
188 | * Removes the packet group from the wireless library.
|
||
189 | **/
|
||
190 | 346 | bcoltin | void wl_token_ring_unregister()
|
191 | { |
||
192 | wl_unregister_packet_group(&wl_token_ring_handler); |
||
193 | 17 | bcoltin | } |
194 | |||
195 | /**
|
||
196 | * Sets the functions that are called when the BOM ought to be
|
||
197 | 397 | emarinel | * turned on or off. This could be used for things such as
|
198 | 17 | bcoltin | * charging stations, which have multiple BOMs.
|
199 | *
|
||
200 | * @param on_function the function to be called when the BOM
|
||
201 | * should be turned on
|
||
202 | * @param off_function the function to be called when the BOM
|
||
203 | * should be turned off
|
||
204 | * @param max_bom_function the function to be called when a
|
||
205 | * measurement of the maximum BOM reading is needed.
|
||
206 | **/
|
||
207 | void wl_token_ring_set_bom_functions(void (*on_function) (void), |
||
208 | 346 | bcoltin | void (*off_function) (void), int (*max_bom_function) (void)) |
209 | { |
||
210 | bom_on_function = on_function; |
||
211 | bom_off_function = off_function; |
||
212 | get_max_bom_function = max_bom_function; |
||
213 | 17 | bcoltin | } |
214 | |||
215 | /**
|
||
216 | * Called to cleanup the token ring packet group.
|
||
217 | **/
|
||
218 | 423 | emarinel | static void wl_token_ring_cleanup() |
219 | 346 | bcoltin | { |
220 | 17 | bcoltin | } |
221 | |||
222 | /**
|
||
223 | * Called approximately every quarter second by the wireless library.
|
||
224 | **/
|
||
225 | 418 | emarinel | static void wl_token_ring_timeout_handler() |
226 | 346 | bcoltin | { |
227 | //someone is not responding, assume they are dead
|
||
228 | if (deathDelay == 0) |
||
229 | { |
||
230 | //pass the token to the next robot if we think someone has died
|
||
231 | //also, declare that person dead, as long as it isn't us
|
||
232 | if (wl_token_next_robot != wl_get_xbee_id())
|
||
233 | { |
||
234 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_token_next_robot, 0);
|
235 | 346 | bcoltin | WL_DEBUG_PRINT("Robot ");
|
236 | WL_DEBUG_PRINT_INT(wl_token_next_robot); |
||
237 | WL_DEBUG_PRINT(" has died.\r\n");
|
||
238 | wl_token_next_robot = -1;
|
||
239 | deathDelay = DEATH_DELAY; |
||
240 | } |
||
241 | 397 | emarinel | |
242 | 346 | bcoltin | // we may have been dropped from the ring when this is received
|
243 | 423 | emarinel | if (ringState == MEMBER) {
|
244 | 346 | bcoltin | wl_token_pass_token(); |
245 | 423 | emarinel | } |
246 | 346 | bcoltin | } |
247 | 17 | bcoltin | |
248 | 346 | bcoltin | //we must start our own token ring, no one is responding to us
|
249 | if (joinDelay == 0) |
||
250 | { |
||
251 | 736 | bcoltin | if (sensor_matrix_get_joined() == 0) |
252 | 346 | bcoltin | { |
253 | WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");
|
||
254 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
|
255 | 346 | bcoltin | ringState = MEMBER; |
256 | //this will make us pass the token to ourself
|
||
257 | //repeatedly, and other robots when they join
|
||
258 | deathDelay = DEATH_DELAY; |
||
259 | wl_token_next_robot = wl_get_xbee_id(); |
||
260 | } |
||
261 | else
|
||
262 | { |
||
263 | WL_DEBUG_PRINT("Attempting to join the token ring again.\r\n");
|
||
264 | //attempt to rejoin with a random delay
|
||
265 | 1452 | dsschult | //TODO: should we use the constant JOIN_DELAY ?
|
266 | 346 | bcoltin | wl_token_ring_join(); |
267 | joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;
|
||
268 | } |
||
269 | } |
||
270 | 17 | bcoltin | |
271 | 423 | emarinel | if (deathDelay >= 0) { |
272 | 346 | bcoltin | deathDelay--; |
273 | 423 | emarinel | } |
274 | |||
275 | if (joinDelay >= 0) { |
||
276 | 346 | bcoltin | joinDelay--; |
277 | 423 | emarinel | } |
278 | |||
279 | if (bom_on_count >= 0) { |
||
280 | 346 | bcoltin | bom_on_count++; |
281 | 423 | emarinel | } |
282 | 17 | bcoltin | } |
283 | |||
284 | /**
|
||
285 | * Called when the XBee tells us if a packet we sent has been received.
|
||
286 | 397 | emarinel | *
|
287 | 17 | bcoltin | * @param frame the frame number assigned when the packet was sent
|
288 | * @param received 1 if the packet was received, 0 otherwise
|
||
289 | **/
|
||
290 | 418 | emarinel | static void wl_token_ring_response_handler(int frame, int received) |
291 | 346 | bcoltin | { |
292 | if (!received)
|
||
293 | { |
||
294 | WL_DEBUG_PRINT("FAILED.\r\n");
|
||
295 | } |
||
296 | 17 | bcoltin | } |
297 | |||
298 | /**
|
||
299 | * Called when we recieve a token ring packet.
|
||
300 | * @param type the type of the packet
|
||
301 | * @param source the id of the robot who sent the packet
|
||
302 | * @param packet the data in the packet
|
||
303 | * @param length the length of the packet in bytes
|
||
304 | **/
|
||
305 | 418 | emarinel | static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length) |
306 | 346 | bcoltin | { |
307 | switch (type)
|
||
308 | { |
||
309 | case WL_TOKEN_PASS:
|
||
310 | 736 | bcoltin | wl_token_pass_receive(source); |
311 | 346 | bcoltin | break;
|
312 | 736 | bcoltin | case WL_TOKEN_SENSOR_MATRIX:
|
313 | wl_token_sensor_matrix_receive(source, packet, length); |
||
314 | break;
|
||
315 | 346 | bcoltin | case WL_TOKEN_BOM_ON:
|
316 | //add the robot to the sensor matrix if it is not already there
|
||
317 | wl_token_bom_on_receive(source); |
||
318 | break;
|
||
319 | case WL_TOKEN_JOIN:
|
||
320 | wl_token_join_receive(source); |
||
321 | break;
|
||
322 | case WL_TOKEN_JOIN_ACCEPT:
|
||
323 | wl_token_join_accept_receive(source); |
||
324 | break;
|
||
325 | default:
|
||
326 | WL_DEBUG_PRINT("Unimplemented token ring packet received.\r\n");
|
||
327 | break;
|
||
328 | } |
||
329 | 17 | bcoltin | } |
330 | |||
331 | /**
|
||
332 | * Returns the BOM reading robot source has for robot dest.
|
||
333 | *
|
||
334 | * @param source the robot that made the BOM reading
|
||
335 | * @param dest the robot whose relative location is returned
|
||
336 | *
|
||
337 | * @return a BOM reading from robot source to robot dest,
|
||
338 | * in the range 0-15, or -1 if it is unknown
|
||
339 | **/
|
||
340 | 346 | bcoltin | int wl_token_get_sensor_reading(int source, int dest) |
341 | { |
||
342 | if (wl_token_is_robot_in_ring(dest) &&
|
||
343 | 418 | emarinel | (source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source))) { |
344 | 736 | bcoltin | return sensor_matrix_get_reading(source, dest);
|
345 | 418 | emarinel | } |
346 | |||
347 | 346 | bcoltin | return -1; |
348 | 17 | bcoltin | } |
349 | |||
350 | /**
|
||
351 | * Returns the BOM reading we have for robot dest.
|
||
352 | 397 | emarinel | *
|
353 | 17 | bcoltin | * @param dest the robot whose relative location is returned
|
354 | *
|
||
355 | * @return a BOM reading from us to robot dest, in the range
|
||
356 | * 0-15, or -1 if it is unkown
|
||
357 | **/
|
||
358 | 346 | bcoltin | int wl_token_get_my_sensor_reading(int dest) |
359 | { |
||
360 | return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
|
||
361 | 17 | bcoltin | } |
362 | |||
363 | 423 | emarinel | |
364 | 17 | bcoltin | /**
|
365 | 423 | emarinel | * Returns the number of robots in the token ring.
|
366 | *
|
||
367 | * @return the number of robots in the token ring
|
||
368 | **/
|
||
369 | int wl_token_get_robots_in_ring(void) |
||
370 | { |
||
371 | 736 | bcoltin | return sensor_matrix_get_joined();
|
372 | 423 | emarinel | } |
373 | |||
374 | /**
|
||
375 | * Returns true if the specified robot is in the token ring, false
|
||
376 | * otherwise.
|
||
377 | *
|
||
378 | * @param robot the robot to check for whether it is in the token ring
|
||
379 | * @return nonzero if the robot is in the token ring, zero otherwise
|
||
380 | **/
|
||
381 | int wl_token_is_robot_in_ring(int robot) |
||
382 | { |
||
383 | 736 | bcoltin | return sensor_matrix_get_in_ring(robot);
|
384 | 423 | emarinel | } |
385 | |||
386 | /**
|
||
387 | * Begins iterating through the robots in the token ring.
|
||
388 | *
|
||
389 | * @see wl_token_iterator_has_next, wl_token_iterator_next
|
||
390 | **/
|
||
391 | void wl_token_iterator_begin(void) |
||
392 | { |
||
393 | int i = 0; |
||
394 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
395 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
396 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
397 | // the value in a variable and check against that variable in the loop condition
|
||
398 | 736 | bcoltin | while (!sensor_matrix_get_in_ring(i) && i < sensor_matrix_get_size()) {
|
399 | 423 | emarinel | i++; |
400 | } |
||
401 | |||
402 | 717 | jknichel | //TODO: if you do the above comment, then you can compare this to the variable also
|
403 | 736 | bcoltin | if (i == sensor_matrix_get_size()) {
|
404 | 423 | emarinel | i = -1;
|
405 | } |
||
406 | |||
407 | iteratorCount = i; |
||
408 | } |
||
409 | |||
410 | /**
|
||
411 | * Returns true if there are more robots in the token ring
|
||
412 | * to iterate through, and false otherwise.
|
||
413 | *
|
||
414 | * @return nonzero if there are more robots to iterate through,
|
||
415 | * zero otherwise
|
||
416 | *
|
||
417 | * @see wl_token_iterator_begin, wl_token_iterator_next
|
||
418 | **/
|
||
419 | int wl_token_iterator_has_next(void) |
||
420 | { |
||
421 | return iteratorCount != -1; |
||
422 | } |
||
423 | |||
424 | /**
|
||
425 | * Returns the next robot ID in the token ring.
|
||
426 | *
|
||
427 | * @return the next robot ID in the token ring, or -1 if none exists
|
||
428 | *
|
||
429 | * @see wl_token_iterator_begin, wl_token_iterator_has_next
|
||
430 | **/
|
||
431 | int wl_token_iterator_next(void) |
||
432 | { |
||
433 | int result = iteratorCount;
|
||
434 | if (result < 0) { |
||
435 | return result;
|
||
436 | } |
||
437 | |||
438 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
439 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
440 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
441 | // the value in a variable and check against that variable in the loop condition
|
||
442 | 423 | emarinel | iteratorCount++; |
443 | 736 | bcoltin | while (!sensor_matrix_get_in_ring(iteratorCount)
|
444 | && iteratorCount < sensor_matrix_get_size()) { |
||
445 | 423 | emarinel | iteratorCount++; |
446 | } |
||
447 | |||
448 | 717 | jknichel | //TODO: if you do the above comment, then you can compare this to the variable also
|
449 | 736 | bcoltin | if (iteratorCount == sensor_matrix_get_size()) {
|
450 | 423 | emarinel | iteratorCount = -1;
|
451 | } |
||
452 | |||
453 | return result;
|
||
454 | } |
||
455 | |||
456 | /**
|
||
457 | * Returns the number of robots currently in the token ring.
|
||
458 | *
|
||
459 | * @return the number of robots in the token ring
|
||
460 | **/
|
||
461 | int wl_token_get_num_robots(void) |
||
462 | { |
||
463 | 736 | bcoltin | return sensor_matrix_get_joined();
|
464 | 423 | emarinel | } |
465 | |||
466 | /**
|
||
467 | * Returns the number of robots in the sensor matrix.
|
||
468 | *
|
||
469 | * @return the number of robots in the sensor matrix
|
||
470 | **/
|
||
471 | int wl_token_get_matrix_size(void) |
||
472 | { |
||
473 | 736 | bcoltin | return sensor_matrix_get_size();
|
474 | 423 | emarinel | } |
475 | |||
476 | /**
|
||
477 | 17 | bcoltin | * This method is called when we receive a token pass packet.
|
478 | 736 | bcoltin | *
|
479 | * @param source the robot who passed the token to us.
|
||
480 | **/
|
||
481 | static void wl_token_pass_receive(int source) |
||
482 | { |
||
483 | WL_DEBUG_PRINT("Received token from ");
|
||
484 | WL_DEBUG_PRINT_INT(source); |
||
485 | WL_DEBUG_PRINT(", expected ");
|
||
486 | WL_DEBUG_PRINT_INT(wl_token_next_robot); |
||
487 | WL_DEBUG_PRINT(".\n");
|
||
488 | // this prevents two tokens from being passed around at a time (second clause is in case we are joining)
|
||
489 | if ((source != wl_token_next_robot && wl_get_xbee_id() != wl_token_next_robot) && bom_on_count <= DEATH_DELAY / 2 && |
||
490 | ringState != ACCEPTED) |
||
491 | { |
||
492 | WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
|
||
493 | WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
|
||
494 | return;
|
||
495 | } |
||
496 | bom_on_count = -1;
|
||
497 | deathDelay = -1;
|
||
498 | sensor_matrix_set_in_ring(source, 1);
|
||
499 | wl_token_get_token(); |
||
500 | } |
||
501 | |||
502 | /**
|
||
503 | * This method is called when we receive a token pass packet.
|
||
504 | 17 | bcoltin | * @param source is the robot it came from
|
505 | * @param nextRobot is the robot the token was passed to
|
||
506 | * @param sensorData a char with an id followed by a char with the sensor
|
||
507 | * reading for that robot, repeated for sensorDataLength bytes
|
||
508 | * @param sensorDataLength the length in bytes of sensorData
|
||
509 | */
|
||
510 | 736 | bcoltin | static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength) |
511 | 346 | bcoltin | { |
512 | int i, j;
|
||
513 | 736 | bcoltin | char nextRobot;
|
514 | 196 | bcoltin | |
515 | 346 | bcoltin | bom_on_count = -1;
|
516 | deathDelay = -1;
|
||
517 | 736 | bcoltin | sensor_matrix_set_in_ring(source, 1);
|
518 | 17 | bcoltin | |
519 | 346 | bcoltin | //with this packet, we are passed the id of the next robot in the ring
|
520 | //and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)
|
||
521 | j = 0;
|
||
522 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
523 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
524 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
525 | // the value in a variable and check against that variable in the loop condition
|
||
526 | 736 | bcoltin | for (i = 0; i < sensor_matrix_get_size(); i++) |
527 | 346 | bcoltin | { |
528 | 423 | emarinel | if (i == source) {
|
529 | 346 | bcoltin | continue;
|
530 | 423 | emarinel | } |
531 | 397 | emarinel | |
532 | 346 | bcoltin | //set the sensor information we receive
|
533 | if (j < sensorDataLength / 2 && sensorData[2 * j] == i) |
||
534 | { |
||
535 | //the robot we were going to accept has already been accepted
|
||
536 | if (accepted == i)
|
||
537 | { |
||
538 | accepted = -1;
|
||
539 | WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
|
||
540 | } |
||
541 | 736 | bcoltin | sensor_matrix_set_reading(source, i, |
542 | 346 | bcoltin | sensorData[2 * j + 1]); |
543 | 736 | bcoltin | if (!sensor_matrix_get_in_ring(i))
|
544 | { |
||
545 | WL_DEBUG_PRINT("Robot ");
|
||
546 | WL_DEBUG_PRINT_INT(i); |
||
547 | WL_DEBUG_PRINT(" has been added to the sensor matrix of robot ");
|
||
548 | WL_DEBUG_PRINT_INT(wl_get_xbee_id()); |
||
549 | WL_DEBUG_PRINT(" due to a packet from robot ");
|
||
550 | WL_DEBUG_PRINT_INT(source); |
||
551 | WL_DEBUG_PRINT(".\r\n");
|
||
552 | } |
||
553 | sensor_matrix_set_in_ring(i, 1);
|
||
554 | 346 | bcoltin | j++; |
555 | } |
||
556 | else
|
||
557 | { |
||
558 | 736 | bcoltin | if (sensor_matrix_get_in_ring(i))
|
559 | 346 | bcoltin | { |
560 | WL_DEBUG_PRINT("Robot ");
|
||
561 | WL_DEBUG_PRINT_INT(i); |
||
562 | WL_DEBUG_PRINT(" has been removed from the sensor matrix of robot ");
|
||
563 | WL_DEBUG_PRINT_INT(wl_get_xbee_id()); |
||
564 | WL_DEBUG_PRINT(" due to a packet from robot ");
|
||
565 | WL_DEBUG_PRINT_INT(source); |
||
566 | WL_DEBUG_PRINT(".\r\n");
|
||
567 | 736 | bcoltin | sensor_matrix_set_in_ring(i, 0);
|
568 | 346 | bcoltin | } |
569 | 17 | bcoltin | |
570 | 346 | bcoltin | if (i == wl_get_xbee_id() && ringState == MEMBER)
|
571 | { |
||
572 | ringState = NONMEMBER; |
||
573 | wl_token_ring_join(); |
||
574 | 397 | emarinel | |
575 | 346 | bcoltin | WL_DEBUG_PRINT("We have been removed from the ring ");
|
576 | WL_DEBUG_PRINT("and are rejoining.\r\n");
|
||
577 | } |
||
578 | 397 | emarinel | |
579 | 346 | bcoltin | //the person who accepted us is dead... let's ask again
|
580 | if (i == acceptor)
|
||
581 | { |
||
582 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
|
583 | 346 | bcoltin | ringState = NONMEMBER; |
584 | acceptor = -1;
|
||
585 | wl_token_ring_join(); |
||
586 | } |
||
587 | } |
||
588 | } |
||
589 | 736 | bcoltin | |
590 | // get the next robot in the token ring
|
||
591 | i = source + 1;
|
||
592 | while (1) |
||
593 | { |
||
594 | if (i == sensor_matrix_get_size()) {
|
||
595 | i = 0;
|
||
596 | } |
||
597 | 17 | bcoltin | |
598 | 736 | bcoltin | if (sensor_matrix_get_in_ring(i) || i == source)
|
599 | { |
||
600 | nextRobot = (char)i;
|
||
601 | break;
|
||
602 | } |
||
603 | 397 | emarinel | |
604 | 736 | bcoltin | i++; |
605 | } |
||
606 | |||
607 | if (nextRobot != wl_get_xbee_id())
|
||
608 | wl_token_next_robot = nextRobot; |
||
609 | |||
610 | 346 | bcoltin | deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY; |
611 | 397 | emarinel | |
612 | 736 | bcoltin | if (sensor_matrix_get_joined() == 0 && ringState == JOINING) |
613 | wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME); |
||
614 | 17 | bcoltin | } |
615 | |||
616 | /**
|
||
617 | * Gets the distance in the token ring between two robots.
|
||
618 | *
|
||
619 | * @param robot1 the first robot
|
||
620 | * @param robot2 the second robot
|
||
621 | *
|
||
622 | * @return the number of passes before the token is expected
|
||
623 | * to reach robot2 from robot1
|
||
624 | **/
|
||
625 | 423 | emarinel | static int get_token_distance(int robot1, int robot2) |
626 | 346 | bcoltin | { |
627 | int curr = robot1 + 1; |
||
628 | int count = 1; |
||
629 | while (1) |
||
630 | { |
||
631 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
632 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
633 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
634 | // the value in a variable and check against that variable in the loop condition
|
||
635 | 736 | bcoltin | if (curr == sensor_matrix_get_size())
|
636 | 346 | bcoltin | curr = 0;
|
637 | if (curr == robot2)
|
||
638 | break;
|
||
639 | 736 | bcoltin | if (sensor_matrix_get_in_ring(curr))
|
640 | 346 | bcoltin | count++; |
641 | curr++; |
||
642 | } |
||
643 | return count;
|
||
644 | 17 | bcoltin | } |
645 | |||
646 | /**
|
||
647 | * Passes the token to the next robot in the token ring.
|
||
648 | **/
|
||
649 | 423 | emarinel | static int wl_token_pass_token() |
650 | 346 | bcoltin | { |
651 | 736 | bcoltin | char nextRobot = 0xFF; |
652 | 346 | bcoltin | int i = wl_get_xbee_id() + 1; |
653 | 736 | bcoltin | char buf[2 * sensor_matrix_get_size()]; |
654 | 346 | bcoltin | if (accepted == -1) |
655 | { |
||
656 | while (1) |
||
657 | { |
||
658 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
659 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
660 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
661 | // the value in a variable and check against that variable in the loop condition
|
||
662 | 736 | bcoltin | if (i == sensor_matrix_get_size()) {
|
663 | 346 | bcoltin | i = 0;
|
664 | 423 | emarinel | } |
665 | |||
666 | 736 | bcoltin | if (sensor_matrix_get_in_ring(i))
|
667 | 346 | bcoltin | { |
668 | nextRobot = (char)i;
|
||
669 | break;
|
||
670 | } |
||
671 | 423 | emarinel | |
672 | 346 | bcoltin | i++; |
673 | } |
||
674 | } |
||
675 | else
|
||
676 | { |
||
677 | WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");
|
||
678 | //add a new robot to the token ring
|
||
679 | 736 | bcoltin | sensor_matrix_set_in_ring(accepted, 1);
|
680 | 346 | bcoltin | nextRobot = accepted; |
681 | accepted = -1;
|
||
682 | } |
||
683 | 17 | bcoltin | |
684 | 346 | bcoltin | int j = 0; |
685 | 717 | jknichel | //TODO: the compiler may or may not optimize this such that my comment is useless:
|
686 | // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
|
||
687 | // the overhead of a function call each iteration, call it only once before the loop and store
|
||
688 | // the value in a variable and check against that variable in the loop condition
|
||
689 | 736 | bcoltin | for (i = 0; i < sensor_matrix_get_size(); i++) { |
690 | if (sensor_matrix_get_in_ring(i) && i != wl_get_xbee_id())
|
||
691 | 346 | bcoltin | { |
692 | 736 | bcoltin | buf[2*j] = i;
|
693 | buf[2*j + 1] = sensor_matrix_get_reading(wl_get_xbee_id(), i); |
||
694 | 346 | bcoltin | j++; |
695 | } |
||
696 | 423 | emarinel | } |
697 | 397 | emarinel | |
698 | 736 | bcoltin | int packetSize = 2 * j * sizeof(char); |
699 | 346 | bcoltin | WL_DEBUG_PRINT("Passing the token to robot ");
|
700 | 736 | bcoltin | WL_DEBUG_PRINT_INT(nextRobot); |
701 | 346 | bcoltin | WL_DEBUG_PRINT(".\r\n");
|
702 | 736 | bcoltin | if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0) |
703 | 423 | emarinel | return -1; |
704 | 736 | bcoltin | if (wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME)) |
705 | return -1; |
||
706 | 17 | bcoltin | |
707 | 346 | bcoltin | wl_token_next_robot = nextRobot; |
708 | deathDelay = DEATH_DELAY; |
||
709 | 423 | emarinel | |
710 | return 0; |
||
711 | 17 | bcoltin | } |
712 | |||
713 | /**
|
||
714 | * Called when a packet is received stating that another robot has turned
|
||
715 | * its BOM on. Our BOM is then read, and the data is added to the sensor
|
||
716 | * matrix.
|
||
717 | *
|
||
718 | * @param source the robot whose BOM is on
|
||
719 | **/
|
||
720 | 423 | emarinel | static void wl_token_bom_on_receive(int source) |
721 | 346 | bcoltin | { |
722 | 1445 | rcahoon | int max;
|
723 | 1443 | rcahoon | |
724 | 346 | bcoltin | WL_DEBUG_PRINT("Robot ");
|
725 | WL_DEBUG_PRINT_INT(source); |
||
726 | WL_DEBUG_PRINT(" has flashed its bom.\r\n");
|
||
727 | 397 | emarinel | |
728 | 346 | bcoltin | bom_on_count = 0;
|
729 | 138 | bcoltin | |
730 | 1443 | rcahoon | max = get_max_bom_function(); |
731 | 736 | bcoltin | sensor_matrix_set_reading(wl_get_xbee_id(), |
732 | 1443 | rcahoon | source, max); |
733 | |||
734 | WL_DEBUG_PRINT("Max: ");
|
||
735 | WL_DEBUG_PRINT_INT(max); |
||
736 | WL_DEBUG_PRINT("\n\n");
|
||
737 | 17 | bcoltin | } |
738 | |||
739 | /**
|
||
740 | * This method is called when we receive the token. Upon receiving
|
||
741 | * the token, we must send a BOM_ON packet, flash the BOM, and send
|
||
742 | * the token to the next robot.
|
||
743 | 397 | emarinel | *
|
744 | 17 | bcoltin | * If there is a pending request for the token, this is processed first.
|
745 | **/
|
||
746 | 423 | emarinel | static void wl_token_get_token() |
747 | 346 | bcoltin | { |
748 | WL_DEBUG_PRINT("We have the token.\r\n");
|
||
749 | if (ringState == ACCEPTED)
|
||
750 | { |
||
751 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
|
752 | 346 | bcoltin | WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
|
753 | ringState = MEMBER; |
||
754 | joinDelay = -1;
|
||
755 | } |
||
756 | 17 | bcoltin | |
757 | 346 | bcoltin | if (ringState == LEAVING || ringState == NONMEMBER)
|
758 | { |
||
759 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
|
760 | 346 | bcoltin | if (ringState == NONMEMBER)
|
761 | { |
||
762 | WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
|
||
763 | } |
||
764 | return;
|
||
765 | } |
||
766 | 397 | emarinel | |
767 | 346 | bcoltin | WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
|
768 | 423 | emarinel | wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON, NULL, 0, 0); |
769 | 17 | bcoltin | |
770 | 346 | bcoltin | bom_on_function(); |
771 | #ifdef ROBOT
|
||
772 | delay_ms(BOM_DELAY); |
||
773 | #endif
|
||
774 | bom_off_function(); |
||
775 | 397 | emarinel | |
776 | 736 | bcoltin | if (!sensor_matrix_get_in_ring(wl_get_xbee_id()))
|
777 | 346 | bcoltin | { |
778 | WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
|
||
779 | return;
|
||
780 | } |
||
781 | 397 | emarinel | |
782 | 346 | bcoltin | wl_token_pass_token(); |
783 | 17 | bcoltin | } |
784 | |||
785 | /**
|
||
786 | * Called when a request to join the token ring is received.
|
||
787 | * If we are the robot preceding the requester in the ring,
|
||
788 | * we respond with a JOIN_ACCEPT packet and pass the token to
|
||
789 | * this robot when we receive the token.
|
||
790 | *
|
||
791 | * @param source the robot who requested to join
|
||
792 | **/
|
||
793 | 423 | emarinel | static void wl_token_join_receive(int source) |
794 | 346 | bcoltin | { |
795 | WL_DEBUG_PRINT("Received joining request from robot ");
|
||
796 | WL_DEBUG_PRINT_INT(source); |
||
797 | WL_DEBUG_PRINT(".\r\n");
|
||
798 | 17 | bcoltin | |
799 | 346 | bcoltin | //we cannot accept the request if we are not a member
|
800 | if (ringState != MEMBER)
|
||
801 | return;
|
||
802 | //if they didn't get our response, see if we should respond again
|
||
803 | if (accepted == source)
|
||
804 | accepted = -1;
|
||
805 | //we can only accept one request at a time
|
||
806 | if (accepted != -1) |
||
807 | return;
|
||
808 | 397 | emarinel | |
809 | 346 | bcoltin | //check if we are the preceding robot in the token ring
|
810 | int i = source - 1; |
||
811 | while (1) |
||
812 | { |
||
813 | if (i < 0) |
||
814 | 736 | bcoltin | i = sensor_matrix_get_size() - 1;
|
815 | 717 | jknichel | |
816 | 346 | bcoltin | //we must send a join acceptance
|
817 | if (i == wl_get_xbee_id())
|
||
818 | break;
|
||
819 | 17 | bcoltin | |
820 | 346 | bcoltin | //another robot will handle it
|
821 | 736 | bcoltin | if (sensor_matrix_get_in_ring(i))
|
822 | 346 | bcoltin | return;
|
823 | 717 | jknichel | |
824 | 346 | bcoltin | i--; |
825 | } |
||
826 | 17 | bcoltin | |
827 | 346 | bcoltin | accepted = source; |
828 | wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT, |
||
829 | NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME); |
||
830 | 397 | emarinel | |
831 | 346 | bcoltin | WL_DEBUG_PRINT("Accepting robot ");
|
832 | WL_DEBUG_PRINT_INT(source); |
||
833 | WL_DEBUG_PRINT(" into the token ring.\r\n");
|
||
834 | 17 | bcoltin | |
835 | 346 | bcoltin | // the token ring has not started yet
|
836 | 736 | bcoltin | if (sensor_matrix_get_joined() == 1) |
837 | 346 | bcoltin | wl_token_pass_token(); |
838 | 17 | bcoltin | } |
839 | |||
840 | /**
|
||
841 | * Called when we receive a JOIN_ACCEPT packet in attempting to join
|
||
842 | * the token ring.
|
||
843 | * Our attempt to join the ring is stopped, and we wait for the token.
|
||
844 | *
|
||
845 | * @param source the robot who accepted us
|
||
846 | **/
|
||
847 | 423 | emarinel | static void wl_token_join_accept_receive(int source) |
848 | 346 | bcoltin | { |
849 | WL_DEBUG_PRINT("Accepted into the token ring by robot ");
|
||
850 | WL_DEBUG_PRINT_INT(source); |
||
851 | WL_DEBUG_PRINT(".\r\n");
|
||
852 | joinDelay = JOIN_DELAY; |
||
853 | ringState = ACCEPTED; |
||
854 | acceptor = source; |
||
855 | 17 | bcoltin | |
856 | 346 | bcoltin | //add ourselves to the token ring
|
857 | 736 | bcoltin | sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
|
858 | 17 | bcoltin | } |