root / trunk / code / projects / libwireless / lib / wl_token_ring.c @ 518
History | View | Annotate | Download (20.5 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 | #include <stdio.h> |
||
39 | |||
40 | #include <wl_defs.h> |
||
41 | #include <wireless.h> |
||
42 | #include <sensor_matrix.h> |
||
43 | |||
44 | #ifdef ROBOT
|
||
45 | 86 | bcoltin | #ifndef FIREFLY
|
46 | 17 | bcoltin | #include <bom.h> |
47 | 86 | bcoltin | #endif
|
48 | 17 | bcoltin | #include <time.h> |
49 | #endif
|
||
50 | |||
51 | #define DEFAULT_SENSOR_MATRIX_SIZE 20 |
||
52 | |||
53 | /*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 | #define TOKEN_JOIN_ACCEPT_FRAME 1 |
||
63 | |||
64 | /*Function Prototypes*/
|
||
65 | |||
66 | /*Wireless Library Prototypes*/
|
||
67 | 418 | emarinel | static void wl_token_ring_timeout_handler(void); |
68 | static void wl_token_ring_response_handler(int frame, int received); |
||
69 | static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length); |
||
70 | static void wl_token_ring_cleanup(void); |
||
71 | 17 | bcoltin | |
72 | /*Helper Functions*/
|
||
73 | 423 | emarinel | static int wl_token_pass_token(void); |
74 | 418 | emarinel | static int get_token_distance(int robot1, int robot2); |
75 | static void wl_token_get_token(void); |
||
76 | 17 | bcoltin | |
77 | /*Packet Handling Routines*/
|
||
78 | 418 | emarinel | static void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength); |
79 | static void wl_token_bom_on_receive(int source); |
||
80 | static void wl_token_join_receive(int source); |
||
81 | static void wl_token_join_accept_receive(int source); |
||
82 | 17 | bcoltin | |
83 | /*Global Variables*/
|
||
84 | |||
85 | //the sensor matrix
|
||
86 | 418 | emarinel | static SensorMatrix* sensorMatrix;
|
87 | 17 | bcoltin | |
88 | //the robot we are waiting to say it has received the token. -1 if unspecified
|
||
89 | 418 | emarinel | static int wl_token_next_robot = -1; |
90 | 17 | bcoltin | |
91 | //true if the robot should be in the token ring, 0 otherwise
|
||
92 | 418 | emarinel | static int ringState = NONMEMBER; |
93 | 17 | bcoltin | //the id of the robot who accepted us into the token ring, only used in ACCEPTED state
|
94 | 418 | emarinel | static int acceptor = -1; |
95 | 17 | bcoltin | //id of the robot we are accepting
|
96 | 418 | emarinel | static int accepted = -1; |
97 | 17 | bcoltin | |
98 | //the counter for when we assume a robot is dead
|
||
99 | 418 | emarinel | static int deathDelay = -1; |
100 | 17 | bcoltin | //the counter for joining, before we form our own token ring
|
101 | 418 | emarinel | static int joinDelay = -1; |
102 | 17 | bcoltin | |
103 | 52 | bcoltin | //current robot to check in the iterator
|
104 | 418 | emarinel | static int iteratorCount = 0; |
105 | 52 | bcoltin | |
106 | 196 | bcoltin | // the amount of time a robot has had its BOM on for
|
107 | 418 | emarinel | static int bom_on_count = 0; |
108 | 196 | bcoltin | |
109 | 418 | emarinel | static void do_nothing(void) {} |
110 | static int get_nothing(void) {return -1;} |
||
111 | 86 | bcoltin | |
112 | 17 | bcoltin | #ifdef ROBOT
|
113 | 86 | bcoltin | #ifndef FIREFLY
|
114 | 418 | emarinel | static void (*bom_on_function) (void) = bom_on; |
115 | static void (*bom_off_function) (void) = bom_off; |
||
116 | static int (*get_max_bom_function) (void) = get_max_bom; |
||
117 | 17 | bcoltin | #else
|
118 | 418 | emarinel | static void (*bom_on_function) (void) = do_nothing; |
119 | static void (*bom_off_function) (void) = do_nothing; |
||
120 | static int (*get_max_bom_function) (void) = get_nothing; |
||
121 | 17 | bcoltin | #endif
|
122 | 86 | bcoltin | #else
|
123 | 418 | emarinel | static void (*bom_on_function) (void) = do_nothing; |
124 | static void (*bom_off_function) (void) = do_nothing; |
||
125 | static int (*get_max_bom_function) (void) = get_nothing; |
||
126 | 86 | bcoltin | #endif
|
127 | 17 | bcoltin | |
128 | 418 | emarinel | static PacketGroupHandler wl_token_ring_handler =
|
129 | {WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler, |
||
130 | 346 | bcoltin | wl_token_ring_response_handler, wl_token_ring_receive_handler, |
131 | wl_token_ring_cleanup}; |
||
132 | 17 | bcoltin | |
133 | /**
|
||
134 | 423 | emarinel | * Causes the robot to join an existing token ring, or create one
|
135 | * if no token ring exists. The token ring uses global and robot to robot
|
||
136 | * packets, and does not rely on any PAN.
|
||
137 | **/
|
||
138 | int wl_token_ring_join()
|
||
139 | { |
||
140 | WL_DEBUG_PRINT("Joining the token ring.\r\n");
|
||
141 | |||
142 | ringState = JOINING; |
||
143 | joinDelay = DEATH_DELAY * 2;
|
||
144 | if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN, NULL, 0, 0) != 0) { |
||
145 | return -1; |
||
146 | } |
||
147 | |||
148 | return 0; |
||
149 | } |
||
150 | |||
151 | /**
|
||
152 | * Causes the robot to leave the token ring. The robot stops
|
||
153 | * alerting others of its location, but continues storing the
|
||
154 | * locations of other robots.
|
||
155 | **/
|
||
156 | void wl_token_ring_leave()
|
||
157 | { |
||
158 | ringState = LEAVING; |
||
159 | } |
||
160 | |||
161 | /**
|
||
162 | 17 | bcoltin | * Initialize the token ring packet group and register it with the
|
163 | * wireless library. The robot will not join a token ring.
|
||
164 | **/
|
||
165 | 418 | emarinel | int wl_token_ring_register()
|
166 | 346 | bcoltin | { |
167 | if (wl_get_xbee_id() > 0xFF) |
||
168 | { |
||
169 | //Note: if this becomes an issue (unlikely), we could limit sensor information
|
||
170 | //to half a byte and use 12 bits for the id
|
||
171 | WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");
|
||
172 | WL_DEBUG_PRINT_INT(wl_get_xbee_id()); |
||
173 | WL_DEBUG_PRINT(".\r\n");
|
||
174 | 418 | emarinel | return -1; |
175 | 346 | bcoltin | } |
176 | 397 | emarinel | |
177 | 346 | bcoltin | sensorMatrix = sensor_matrix_create(); |
178 | //add ourselves to the sensor matrix
|
||
179 | sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
|
||
180 | 17 | bcoltin | |
181 | 346 | bcoltin | wl_register_packet_group(&wl_token_ring_handler); |
182 | 418 | emarinel | |
183 | return 0; |
||
184 | 17 | bcoltin | } |
185 | |||
186 | /**
|
||
187 | * Removes the packet group from the wireless library.
|
||
188 | **/
|
||
189 | 346 | bcoltin | void wl_token_ring_unregister()
|
190 | { |
||
191 | wl_unregister_packet_group(&wl_token_ring_handler); |
||
192 | 17 | bcoltin | } |
193 | |||
194 | /**
|
||
195 | * Sets the functions that are called when the BOM ought to be
|
||
196 | 397 | emarinel | * turned on or off. This could be used for things such as
|
197 | 17 | bcoltin | * charging stations, which have multiple BOMs.
|
198 | *
|
||
199 | * @param on_function the function to be called when the BOM
|
||
200 | * should be turned on
|
||
201 | * @param off_function the function to be called when the BOM
|
||
202 | * should be turned off
|
||
203 | * @param max_bom_function the function to be called when a
|
||
204 | * measurement of the maximum BOM reading is needed.
|
||
205 | **/
|
||
206 | void wl_token_ring_set_bom_functions(void (*on_function) (void), |
||
207 | 346 | bcoltin | void (*off_function) (void), int (*max_bom_function) (void)) |
208 | { |
||
209 | bom_on_function = on_function; |
||
210 | bom_off_function = off_function; |
||
211 | get_max_bom_function = max_bom_function; |
||
212 | 17 | bcoltin | } |
213 | |||
214 | /**
|
||
215 | * Called to cleanup the token ring packet group.
|
||
216 | **/
|
||
217 | 423 | emarinel | static void wl_token_ring_cleanup() |
218 | 346 | bcoltin | { |
219 | sensor_matrix_destroy(sensorMatrix); |
||
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 | sensor_matrix_set_in_ring(sensorMatrix, wl_token_next_robot, 0);
|
||
235 | 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 | if (sensor_matrix_get_joined(sensorMatrix) == 0) |
||
252 | { |
||
253 | WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");
|
||
254 | sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
|
||
255 | 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 | wl_token_ring_join(); |
||
266 | joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;
|
||
267 | } |
||
268 | } |
||
269 | 17 | bcoltin | |
270 | 423 | emarinel | if (deathDelay >= 0) { |
271 | 346 | bcoltin | deathDelay--; |
272 | 423 | emarinel | } |
273 | |||
274 | if (joinDelay >= 0) { |
||
275 | 346 | bcoltin | joinDelay--; |
276 | 423 | emarinel | } |
277 | |||
278 | if (bom_on_count >= 0) { |
||
279 | 346 | bcoltin | bom_on_count++; |
280 | 423 | emarinel | } |
281 | 17 | bcoltin | } |
282 | |||
283 | /**
|
||
284 | * Called when the XBee tells us if a packet we sent has been received.
|
||
285 | 397 | emarinel | *
|
286 | 17 | bcoltin | * @param frame the frame number assigned when the packet was sent
|
287 | * @param received 1 if the packet was received, 0 otherwise
|
||
288 | **/
|
||
289 | 418 | emarinel | static void wl_token_ring_response_handler(int frame, int received) |
290 | 346 | bcoltin | { |
291 | if (!received)
|
||
292 | { |
||
293 | WL_DEBUG_PRINT("FAILED.\r\n");
|
||
294 | } |
||
295 | 17 | bcoltin | } |
296 | |||
297 | /**
|
||
298 | * Called when we recieve a token ring packet.
|
||
299 | * @param type the type of the packet
|
||
300 | * @param source the id of the robot who sent the packet
|
||
301 | * @param packet the data in the packet
|
||
302 | * @param length the length of the packet in bytes
|
||
303 | **/
|
||
304 | 418 | emarinel | static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length) |
305 | 346 | bcoltin | { |
306 | switch (type)
|
||
307 | { |
||
308 | case WL_TOKEN_PASS:
|
||
309 | if (length < 1) |
||
310 | { |
||
311 | WL_DEBUG_PRINT("Malformed Token Pass packet received.\r\n");
|
||
312 | return;
|
||
313 | } |
||
314 | wl_token_pass_receive(source, packet[0], packet + 1, length - 1); |
||
315 | break;
|
||
316 | case WL_TOKEN_BOM_ON:
|
||
317 | //add the robot to the sensor matrix if it is not already there
|
||
318 | wl_token_bom_on_receive(source); |
||
319 | break;
|
||
320 | case WL_TOKEN_JOIN:
|
||
321 | wl_token_join_receive(source); |
||
322 | break;
|
||
323 | case WL_TOKEN_JOIN_ACCEPT:
|
||
324 | wl_token_join_accept_receive(source); |
||
325 | break;
|
||
326 | default:
|
||
327 | WL_DEBUG_PRINT("Unimplemented token ring packet received.\r\n");
|
||
328 | break;
|
||
329 | } |
||
330 | 17 | bcoltin | } |
331 | |||
332 | /**
|
||
333 | * Returns the BOM reading robot source has for robot dest.
|
||
334 | *
|
||
335 | * @param source the robot that made the BOM reading
|
||
336 | * @param dest the robot whose relative location is returned
|
||
337 | *
|
||
338 | * @return a BOM reading from robot source to robot dest,
|
||
339 | * in the range 0-15, or -1 if it is unknown
|
||
340 | **/
|
||
341 | 346 | bcoltin | int wl_token_get_sensor_reading(int source, int dest) |
342 | { |
||
343 | if (wl_token_is_robot_in_ring(dest) &&
|
||
344 | 418 | emarinel | (source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source))) { |
345 | 346 | bcoltin | return sensor_matrix_get_reading(sensorMatrix, source, dest);
|
346 | 418 | emarinel | } |
347 | |||
348 | 346 | bcoltin | return -1; |
349 | 17 | bcoltin | } |
350 | |||
351 | /**
|
||
352 | * Returns the BOM reading we have for robot dest.
|
||
353 | 397 | emarinel | *
|
354 | 17 | bcoltin | * @param dest the robot whose relative location is returned
|
355 | *
|
||
356 | * @return a BOM reading from us to robot dest, in the range
|
||
357 | * 0-15, or -1 if it is unkown
|
||
358 | **/
|
||
359 | 346 | bcoltin | int wl_token_get_my_sensor_reading(int dest) |
360 | { |
||
361 | return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
|
||
362 | 17 | bcoltin | } |
363 | |||
364 | 423 | emarinel | |
365 | 17 | bcoltin | /**
|
366 | 423 | emarinel | * Returns the number of robots in the token ring.
|
367 | *
|
||
368 | * @return the number of robots in the token ring
|
||
369 | **/
|
||
370 | int wl_token_get_robots_in_ring(void) |
||
371 | { |
||
372 | return sensor_matrix_get_joined(sensorMatrix);
|
||
373 | } |
||
374 | |||
375 | /**
|
||
376 | * Returns true if the specified robot is in the token ring, false
|
||
377 | * otherwise.
|
||
378 | *
|
||
379 | * @param robot the robot to check for whether it is in the token ring
|
||
380 | * @return nonzero if the robot is in the token ring, zero otherwise
|
||
381 | **/
|
||
382 | int wl_token_is_robot_in_ring(int robot) |
||
383 | { |
||
384 | return sensor_matrix_get_in_ring(sensorMatrix, robot);
|
||
385 | } |
||
386 | |||
387 | /**
|
||
388 | * Begins iterating through the robots in the token ring.
|
||
389 | *
|
||
390 | * @see wl_token_iterator_has_next, wl_token_iterator_next
|
||
391 | **/
|
||
392 | void wl_token_iterator_begin(void) |
||
393 | { |
||
394 | int i = 0; |
||
395 | |||
396 | while (!sensor_matrix_get_in_ring(sensorMatrix, i) && i < sensor_matrix_get_size(sensorMatrix)) {
|
||
397 | i++; |
||
398 | } |
||
399 | |||
400 | if (i == sensor_matrix_get_size(sensorMatrix)) {
|
||
401 | i = -1;
|
||
402 | } |
||
403 | |||
404 | iteratorCount = i; |
||
405 | } |
||
406 | |||
407 | /**
|
||
408 | * Returns true if there are more robots in the token ring
|
||
409 | * to iterate through, and false otherwise.
|
||
410 | *
|
||
411 | * @return nonzero if there are more robots to iterate through,
|
||
412 | * zero otherwise
|
||
413 | *
|
||
414 | * @see wl_token_iterator_begin, wl_token_iterator_next
|
||
415 | **/
|
||
416 | int wl_token_iterator_has_next(void) |
||
417 | { |
||
418 | return iteratorCount != -1; |
||
419 | } |
||
420 | |||
421 | /**
|
||
422 | * Returns the next robot ID in the token ring.
|
||
423 | *
|
||
424 | * @return the next robot ID in the token ring, or -1 if none exists
|
||
425 | *
|
||
426 | * @see wl_token_iterator_begin, wl_token_iterator_has_next
|
||
427 | **/
|
||
428 | int wl_token_iterator_next(void) |
||
429 | { |
||
430 | int result = iteratorCount;
|
||
431 | if (result < 0) { |
||
432 | return result;
|
||
433 | } |
||
434 | |||
435 | iteratorCount++; |
||
436 | while (!sensor_matrix_get_in_ring(sensorMatrix, iteratorCount)
|
||
437 | && iteratorCount < sensor_matrix_get_size(sensorMatrix)) { |
||
438 | iteratorCount++; |
||
439 | } |
||
440 | |||
441 | if (iteratorCount == sensor_matrix_get_size(sensorMatrix)) {
|
||
442 | iteratorCount = -1;
|
||
443 | } |
||
444 | |||
445 | return result;
|
||
446 | } |
||
447 | |||
448 | /**
|
||
449 | * Returns the number of robots currently in the token ring.
|
||
450 | *
|
||
451 | * @return the number of robots in the token ring
|
||
452 | **/
|
||
453 | int wl_token_get_num_robots(void) |
||
454 | { |
||
455 | return sensor_matrix_get_joined(sensorMatrix);
|
||
456 | } |
||
457 | |||
458 | /**
|
||
459 | * Returns the number of robots in the sensor matrix.
|
||
460 | *
|
||
461 | * @return the number of robots in the sensor matrix
|
||
462 | **/
|
||
463 | int wl_token_get_matrix_size(void) |
||
464 | { |
||
465 | return sensor_matrix_get_size(sensorMatrix);
|
||
466 | } |
||
467 | |||
468 | /**
|
||
469 | 17 | bcoltin | * This method is called when we receive a token pass packet.
|
470 | * @param source is the robot it came from
|
||
471 | * @param nextRobot is the robot the token was passed to
|
||
472 | * @param sensorData a char with an id followed by a char with the sensor
|
||
473 | * reading for that robot, repeated for sensorDataLength bytes
|
||
474 | * @param sensorDataLength the length in bytes of sensorData
|
||
475 | */
|
||
476 | 418 | emarinel | static void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength) |
477 | 346 | bcoltin | { |
478 | int i, j;
|
||
479 | 196 | bcoltin | |
480 | 346 | bcoltin | // this prevents two tokens from being passed around at a time (second clause is in case we are joining)
|
481 | if (source != wl_token_next_robot && bom_on_count <= DEATH_DELAY / 2 && |
||
482 | ringState != ACCEPTED) |
||
483 | { |
||
484 | WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
|
||
485 | WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
|
||
486 | return;
|
||
487 | } |
||
488 | 196 | bcoltin | |
489 | 346 | bcoltin | bom_on_count = -1;
|
490 | deathDelay = -1;
|
||
491 | WL_DEBUG_PRINT("Received the token from robot");
|
||
492 | WL_DEBUG_PRINT_INT(source); |
||
493 | WL_DEBUG_PRINT(", next robot is ");
|
||
494 | WL_DEBUG_PRINT_INT((int)nextRobot);
|
||
495 | WL_DEBUG_PRINT(" \r\n");
|
||
496 | sensor_matrix_set_in_ring(sensorMatrix, source, 1);
|
||
497 | 17 | bcoltin | |
498 | 346 | bcoltin | //with this packet, we are passed the id of the next robot in the ring
|
499 | //and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)
|
||
500 | j = 0;
|
||
501 | for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++) |
||
502 | { |
||
503 | 423 | emarinel | if (i == source) {
|
504 | 346 | bcoltin | continue;
|
505 | 423 | emarinel | } |
506 | 397 | emarinel | |
507 | 346 | bcoltin | //set the sensor information we receive
|
508 | if (j < sensorDataLength / 2 && sensorData[2 * j] == i) |
||
509 | { |
||
510 | //the robot we were going to accept has already been accepted
|
||
511 | if (accepted == i)
|
||
512 | { |
||
513 | accepted = -1;
|
||
514 | WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
|
||
515 | } |
||
516 | sensor_matrix_set_reading(sensorMatrix, source, i, |
||
517 | sensorData[2 * j + 1]); |
||
518 | sensor_matrix_set_in_ring(sensorMatrix, i, 1);
|
||
519 | j++; |
||
520 | } |
||
521 | else
|
||
522 | { |
||
523 | if (sensor_matrix_get_in_ring(sensorMatrix, i))
|
||
524 | { |
||
525 | WL_DEBUG_PRINT("Robot ");
|
||
526 | WL_DEBUG_PRINT_INT(i); |
||
527 | WL_DEBUG_PRINT(" has been removed from the sensor matrix of robot ");
|
||
528 | WL_DEBUG_PRINT_INT(wl_get_xbee_id()); |
||
529 | WL_DEBUG_PRINT(" due to a packet from robot ");
|
||
530 | WL_DEBUG_PRINT_INT(source); |
||
531 | WL_DEBUG_PRINT(".\r\n");
|
||
532 | sensor_matrix_set_in_ring(sensorMatrix, i, 0);
|
||
533 | } |
||
534 | 17 | bcoltin | |
535 | 346 | bcoltin | if (i == wl_get_xbee_id() && ringState == MEMBER)
|
536 | { |
||
537 | ringState = NONMEMBER; |
||
538 | wl_token_ring_join(); |
||
539 | 397 | emarinel | |
540 | 346 | bcoltin | WL_DEBUG_PRINT("We have been removed from the ring ");
|
541 | WL_DEBUG_PRINT("and are rejoining.\r\n");
|
||
542 | } |
||
543 | 397 | emarinel | |
544 | 346 | bcoltin | //the person who accepted us is dead... let's ask again
|
545 | if (i == acceptor)
|
||
546 | { |
||
547 | sensor_matrix_set_in_ring(sensorMatrix, |
||
548 | wl_get_xbee_id(), 1);
|
||
549 | ringState = NONMEMBER; |
||
550 | acceptor = -1;
|
||
551 | wl_token_ring_join(); |
||
552 | } |
||
553 | } |
||
554 | } |
||
555 | 17 | bcoltin | |
556 | 346 | bcoltin | wl_token_next_robot = nextRobot; |
557 | 397 | emarinel | |
558 | 346 | bcoltin | deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY; |
559 | 397 | emarinel | |
560 | 346 | bcoltin | //we have the token
|
561 | 423 | emarinel | if (wl_token_next_robot == wl_get_xbee_id()) {
|
562 | 346 | bcoltin | wl_token_get_token(); |
563 | 423 | emarinel | } |
564 | 17 | bcoltin | } |
565 | |||
566 | /**
|
||
567 | * Gets the distance in the token ring between two robots.
|
||
568 | *
|
||
569 | * @param robot1 the first robot
|
||
570 | * @param robot2 the second robot
|
||
571 | *
|
||
572 | * @return the number of passes before the token is expected
|
||
573 | * to reach robot2 from robot1
|
||
574 | **/
|
||
575 | 423 | emarinel | static int get_token_distance(int robot1, int robot2) |
576 | 346 | bcoltin | { |
577 | int curr = robot1 + 1; |
||
578 | int count = 1; |
||
579 | while (1) |
||
580 | { |
||
581 | if (curr == sensor_matrix_get_size(sensorMatrix))
|
||
582 | curr = 0;
|
||
583 | if (curr == robot2)
|
||
584 | break;
|
||
585 | if (sensor_matrix_get_in_ring(sensorMatrix, curr))
|
||
586 | count++; |
||
587 | curr++; |
||
588 | } |
||
589 | return count;
|
||
590 | 17 | bcoltin | } |
591 | |||
592 | /**
|
||
593 | * Passes the token to the next robot in the token ring.
|
||
594 | **/
|
||
595 | 423 | emarinel | static int wl_token_pass_token() |
596 | 346 | bcoltin | { |
597 | char nextRobot;
|
||
598 | int i = wl_get_xbee_id() + 1; |
||
599 | if (accepted == -1) |
||
600 | { |
||
601 | while (1) |
||
602 | { |
||
603 | 423 | emarinel | if (i == sensor_matrix_get_size(sensorMatrix)) {
|
604 | 346 | bcoltin | i = 0;
|
605 | 423 | emarinel | } |
606 | |||
607 | 346 | bcoltin | if (sensor_matrix_get_in_ring(sensorMatrix, i))
|
608 | { |
||
609 | nextRobot = (char)i;
|
||
610 | break;
|
||
611 | } |
||
612 | 423 | emarinel | |
613 | 346 | bcoltin | i++; |
614 | } |
||
615 | } |
||
616 | else
|
||
617 | { |
||
618 | WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");
|
||
619 | //add a new robot to the token ring
|
||
620 | sensor_matrix_set_in_ring(sensorMatrix, accepted, 1);
|
||
621 | nextRobot = accepted; |
||
622 | accepted = -1;
|
||
623 | } |
||
624 | 17 | bcoltin | |
625 | 346 | bcoltin | //we don't include ourself
|
626 | int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1); |
||
627 | char* buf = (char*)malloc(packetSize * sizeof(char)); |
||
628 | if (!buf)
|
||
629 | { |
||
630 | WL_DEBUG_PRINT_INT(packetSize); |
||
631 | WL_DEBUG_PRINT("Out of memory - pass token.\r\n");
|
||
632 | 423 | emarinel | free(buf); |
633 | return -1; |
||
634 | 346 | bcoltin | } |
635 | buf[0] = nextRobot;
|
||
636 | 17 | bcoltin | |
637 | 346 | bcoltin | int j = 0; |
638 | 423 | emarinel | for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++) { |
639 | 346 | bcoltin | if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
|
640 | { |
||
641 | buf[2*j + 1] = i; |
||
642 | buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i); |
||
643 | j++; |
||
644 | } |
||
645 | 423 | emarinel | } |
646 | 397 | emarinel | |
647 | 346 | bcoltin | WL_DEBUG_PRINT("Passing the token to robot ");
|
648 | WL_DEBUG_PRINT_INT(buf[0]);
|
||
649 | WL_DEBUG_PRINT(".\r\n");
|
||
650 | 423 | emarinel | if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, buf, packetSize, 3) != 0) { |
651 | free(buf); |
||
652 | return -1; |
||
653 | } |
||
654 | 17 | bcoltin | |
655 | 346 | bcoltin | wl_token_next_robot = nextRobot; |
656 | deathDelay = DEATH_DELAY; |
||
657 | free(buf); |
||
658 | 423 | emarinel | |
659 | return 0; |
||
660 | 17 | bcoltin | } |
661 | |||
662 | /**
|
||
663 | * Called when a packet is received stating that another robot has turned
|
||
664 | * its BOM on. Our BOM is then read, and the data is added to the sensor
|
||
665 | * matrix.
|
||
666 | *
|
||
667 | * @param source the robot whose BOM is on
|
||
668 | **/
|
||
669 | 423 | emarinel | static void wl_token_bom_on_receive(int source) |
670 | 346 | bcoltin | { |
671 | WL_DEBUG_PRINT("Robot ");
|
||
672 | WL_DEBUG_PRINT_INT(source); |
||
673 | WL_DEBUG_PRINT(" has flashed its bom.\r\n");
|
||
674 | 397 | emarinel | |
675 | 346 | bcoltin | bom_on_count = 0;
|
676 | 138 | bcoltin | |
677 | 397 | emarinel | sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(), |
678 | 346 | bcoltin | source, get_max_bom_function()); |
679 | 17 | bcoltin | } |
680 | |||
681 | /**
|
||
682 | * This method is called when we receive the token. Upon receiving
|
||
683 | * the token, we must send a BOM_ON packet, flash the BOM, and send
|
||
684 | * the token to the next robot.
|
||
685 | 397 | emarinel | *
|
686 | 17 | bcoltin | * If there is a pending request for the token, this is processed first.
|
687 | **/
|
||
688 | 423 | emarinel | static void wl_token_get_token() |
689 | 346 | bcoltin | { |
690 | WL_DEBUG_PRINT("We have the token.\r\n");
|
||
691 | if (ringState == ACCEPTED)
|
||
692 | { |
||
693 | 423 | emarinel | sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
|
694 | 346 | bcoltin | WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
|
695 | ringState = MEMBER; |
||
696 | joinDelay = -1;
|
||
697 | } |
||
698 | 17 | bcoltin | |
699 | 346 | bcoltin | if (ringState == LEAVING || ringState == NONMEMBER)
|
700 | { |
||
701 | sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
|
||
702 | if (ringState == NONMEMBER)
|
||
703 | { |
||
704 | WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
|
||
705 | } |
||
706 | return;
|
||
707 | } |
||
708 | 397 | emarinel | |
709 | 346 | bcoltin | WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
|
710 | 423 | emarinel | wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON, NULL, 0, 0); |
711 | 17 | bcoltin | |
712 | 346 | bcoltin | bom_on_function(); |
713 | #ifdef ROBOT
|
||
714 | delay_ms(BOM_DELAY); |
||
715 | #endif
|
||
716 | bom_off_function(); |
||
717 | 397 | emarinel | |
718 | 346 | bcoltin | if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
|
719 | { |
||
720 | WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
|
||
721 | return;
|
||
722 | } |
||
723 | 397 | emarinel | |
724 | 346 | bcoltin | wl_token_pass_token(); |
725 | 17 | bcoltin | } |
726 | |||
727 | /**
|
||
728 | * Called when a request to join the token ring is received.
|
||
729 | * If we are the robot preceding the requester in the ring,
|
||
730 | * we respond with a JOIN_ACCEPT packet and pass the token to
|
||
731 | * this robot when we receive the token.
|
||
732 | *
|
||
733 | * @param source the robot who requested to join
|
||
734 | **/
|
||
735 | 423 | emarinel | static void wl_token_join_receive(int source) |
736 | 346 | bcoltin | { |
737 | WL_DEBUG_PRINT("Received joining request from robot ");
|
||
738 | WL_DEBUG_PRINT_INT(source); |
||
739 | WL_DEBUG_PRINT(".\r\n");
|
||
740 | 17 | bcoltin | |
741 | 346 | bcoltin | //we cannot accept the request if we are not a member
|
742 | if (ringState != MEMBER)
|
||
743 | return;
|
||
744 | //if they didn't get our response, see if we should respond again
|
||
745 | if (accepted == source)
|
||
746 | accepted = -1;
|
||
747 | //we can only accept one request at a time
|
||
748 | if (accepted != -1) |
||
749 | return;
|
||
750 | 397 | emarinel | |
751 | 346 | bcoltin | //check if we are the preceding robot in the token ring
|
752 | int i = source - 1; |
||
753 | while (1) |
||
754 | { |
||
755 | if (i < 0) |
||
756 | i = sensor_matrix_get_size(sensorMatrix) - 1;
|
||
757 | //we must send a join acceptance
|
||
758 | if (i == wl_get_xbee_id())
|
||
759 | break;
|
||
760 | 17 | bcoltin | |
761 | 346 | bcoltin | //another robot will handle it
|
762 | if (sensor_matrix_get_in_ring(sensorMatrix, i))
|
||
763 | return;
|
||
764 | i--; |
||
765 | } |
||
766 | 17 | bcoltin | |
767 | 346 | bcoltin | accepted = source; |
768 | wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT, |
||
769 | NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME); |
||
770 | 397 | emarinel | |
771 | 346 | bcoltin | WL_DEBUG_PRINT("Accepting robot ");
|
772 | WL_DEBUG_PRINT_INT(source); |
||
773 | WL_DEBUG_PRINT(" into the token ring.\r\n");
|
||
774 | 17 | bcoltin | |
775 | 346 | bcoltin | // the token ring has not started yet
|
776 | if (sensor_matrix_get_joined(sensorMatrix) == 1) |
||
777 | wl_token_pass_token(); |
||
778 | 17 | bcoltin | } |
779 | |||
780 | /**
|
||
781 | * Called when we receive a JOIN_ACCEPT packet in attempting to join
|
||
782 | * the token ring.
|
||
783 | * Our attempt to join the ring is stopped, and we wait for the token.
|
||
784 | *
|
||
785 | * @param source the robot who accepted us
|
||
786 | **/
|
||
787 | 423 | emarinel | static void wl_token_join_accept_receive(int source) |
788 | 346 | bcoltin | { |
789 | WL_DEBUG_PRINT("Accepted into the token ring by robot ");
|
||
790 | WL_DEBUG_PRINT_INT(source); |
||
791 | WL_DEBUG_PRINT(".\r\n");
|
||
792 | joinDelay = JOIN_DELAY; |
||
793 | ringState = ACCEPTED; |
||
794 | acceptor = source; |
||
795 | 17 | bcoltin | |
796 | 346 | bcoltin | //add ourselves to the token ring
|
797 | sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
|
||
798 | 17 | bcoltin | } |