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