Revision 423
fixed robot slave and cleaned up some wireless stuff
wl_token_ring.c | ||
---|---|---|
70 | 70 |
static void wl_token_ring_cleanup(void); |
71 | 71 |
|
72 | 72 |
/*Helper Functions*/ |
73 |
static void wl_token_pass_token(void);
|
|
73 |
static int wl_token_pass_token(void);
|
|
74 | 74 |
static int get_token_distance(int robot1, int robot2); |
75 | 75 |
static void wl_token_get_token(void); |
76 | 76 |
|
... | ... | |
131 | 131 |
wl_token_ring_cleanup}; |
132 | 132 |
|
133 | 133 |
/** |
134 |
* 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 |
/** |
|
134 | 162 |
* Initialize the token ring packet group and register it with the |
135 | 163 |
* wireless library. The robot will not join a token ring. |
136 | 164 |
**/ |
... | ... | |
186 | 214 |
/** |
187 | 215 |
* Called to cleanup the token ring packet group. |
188 | 216 |
**/ |
189 |
void wl_token_ring_cleanup() |
|
217 |
static void wl_token_ring_cleanup()
|
|
190 | 218 |
{ |
191 | 219 |
sensor_matrix_destroy(sensorMatrix); |
192 | 220 |
} |
... | ... | |
212 | 240 |
} |
213 | 241 |
|
214 | 242 |
// we may have been dropped from the ring when this is received |
215 |
if (ringState == MEMBER) |
|
243 |
if (ringState == MEMBER) {
|
|
216 | 244 |
wl_token_pass_token(); |
245 |
} |
|
217 | 246 |
} |
218 | 247 |
|
219 | 248 |
//we must start our own token ring, no one is responding to us |
... | ... | |
238 | 267 |
} |
239 | 268 |
} |
240 | 269 |
|
241 |
if (deathDelay >= 0) |
|
270 |
if (deathDelay >= 0) {
|
|
242 | 271 |
deathDelay--; |
243 |
if (joinDelay >= 0) |
|
272 |
} |
|
273 |
|
|
274 |
if (joinDelay >= 0) { |
|
244 | 275 |
joinDelay--; |
245 |
if (bom_on_count >= 0) |
|
276 |
} |
|
277 |
|
|
278 |
if (bom_on_count >= 0) { |
|
246 | 279 |
bom_on_count++; |
280 |
} |
|
247 | 281 |
} |
248 | 282 |
|
249 | 283 |
/** |
... | ... | |
296 | 330 |
} |
297 | 331 |
|
298 | 332 |
/** |
299 |
* Causes the robot to join an existing token ring, or create one |
|
300 |
* if no token ring exists. The token ring uses global and robot to robot |
|
301 |
* packets, and does not rely on any PAN. |
|
302 |
**/ |
|
303 |
int wl_token_ring_join() |
|
304 |
{ |
|
305 |
WL_DEBUG_PRINT("Joining the token ring.\r\n"); |
|
306 |
ringState = JOINING; |
|
307 |
joinDelay = DEATH_DELAY * 2; |
|
308 |
if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN, NULL, 0, 0) != 0) { |
|
309 |
return -1; |
|
310 |
} |
|
311 |
|
|
312 |
return 0; |
|
313 |
} |
|
314 |
|
|
315 |
/** |
|
316 |
* Causes the robot to leave the token ring. The robot stops |
|
317 |
* alerting others of its location, but continues storing the |
|
318 |
* locations of other robots. |
|
319 |
**/ |
|
320 |
void wl_token_ring_leave() |
|
321 |
{ |
|
322 |
ringState = LEAVING; |
|
323 |
} |
|
324 |
|
|
325 |
/** |
|
326 | 333 |
* Returns the BOM reading robot source has for robot dest. |
327 | 334 |
* |
328 | 335 |
* @param source the robot that made the BOM reading |
... | ... | |
354 | 361 |
return wl_token_get_sensor_reading(wl_get_xbee_id(), dest); |
355 | 362 |
} |
356 | 363 |
|
364 |
|
|
357 | 365 |
/** |
366 |
* 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 |
/** |
|
358 | 469 |
* This method is called when we receive a token pass packet. |
359 | 470 |
* @param source is the robot it came from |
360 | 471 |
* @param nextRobot is the robot the token was passed to |
... | ... | |
389 | 500 |
j = 0; |
390 | 501 |
for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++) |
391 | 502 |
{ |
392 |
if (i == source) |
|
503 |
if (i == source) {
|
|
393 | 504 |
continue; |
505 |
} |
|
394 | 506 |
|
395 | 507 |
//set the sensor information we receive |
396 | 508 |
if (j < sensorDataLength / 2 && sensorData[2 * j] == i) |
... | ... | |
446 | 558 |
deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY; |
447 | 559 |
|
448 | 560 |
//we have the token |
449 |
if (wl_token_next_robot == wl_get_xbee_id()) |
|
561 |
if (wl_token_next_robot == wl_get_xbee_id()) {
|
|
450 | 562 |
wl_token_get_token(); |
563 |
} |
|
451 | 564 |
} |
452 | 565 |
|
453 | 566 |
/** |
... | ... | |
459 | 572 |
* @return the number of passes before the token is expected |
460 | 573 |
* to reach robot2 from robot1 |
461 | 574 |
**/ |
462 |
int get_token_distance(int robot1, int robot2) |
|
575 |
static int get_token_distance(int robot1, int robot2)
|
|
463 | 576 |
{ |
464 | 577 |
int curr = robot1 + 1; |
465 | 578 |
int count = 1; |
... | ... | |
479 | 592 |
/** |
480 | 593 |
* Passes the token to the next robot in the token ring. |
481 | 594 |
**/ |
482 |
void wl_token_pass_token()
|
|
595 |
static int wl_token_pass_token()
|
|
483 | 596 |
{ |
484 | 597 |
char nextRobot; |
485 | 598 |
int i = wl_get_xbee_id() + 1; |
... | ... | |
487 | 600 |
{ |
488 | 601 |
while (1) |
489 | 602 |
{ |
490 |
if (i == sensor_matrix_get_size(sensorMatrix)) |
|
603 |
if (i == sensor_matrix_get_size(sensorMatrix)) {
|
|
491 | 604 |
i = 0; |
605 |
} |
|
606 |
|
|
492 | 607 |
if (sensor_matrix_get_in_ring(sensorMatrix, i)) |
493 | 608 |
{ |
494 | 609 |
nextRobot = (char)i; |
495 | 610 |
break; |
496 | 611 |
} |
612 |
|
|
497 | 613 |
i++; |
498 | 614 |
} |
499 | 615 |
} |
... | ... | |
513 | 629 |
{ |
514 | 630 |
WL_DEBUG_PRINT_INT(packetSize); |
515 | 631 |
WL_DEBUG_PRINT("Out of memory - pass token.\r\n"); |
516 |
return; |
|
632 |
free(buf); |
|
633 |
return -1; |
|
517 | 634 |
} |
518 | 635 |
buf[0] = nextRobot; |
519 | 636 |
|
520 | 637 |
int j = 0; |
521 |
for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++) |
|
638 |
for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++) {
|
|
522 | 639 |
if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id()) |
523 | 640 |
{ |
524 | 641 |
buf[2*j + 1] = i; |
525 | 642 |
buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i); |
526 | 643 |
j++; |
527 | 644 |
} |
645 |
} |
|
528 | 646 |
|
529 | 647 |
WL_DEBUG_PRINT("Passing the token to robot "); |
530 | 648 |
WL_DEBUG_PRINT_INT(buf[0]); |
531 | 649 |
WL_DEBUG_PRINT(".\r\n"); |
532 |
wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, |
|
533 |
buf, packetSize, 3); |
|
650 |
if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, buf, packetSize, 3) != 0) { |
|
651 |
free(buf); |
|
652 |
return -1; |
|
653 |
} |
|
534 | 654 |
|
535 | 655 |
wl_token_next_robot = nextRobot; |
536 | 656 |
deathDelay = DEATH_DELAY; |
537 | 657 |
free(buf); |
658 |
|
|
659 |
return 0; |
|
538 | 660 |
} |
539 | 661 |
|
540 | 662 |
/** |
... | ... | |
544 | 666 |
* |
545 | 667 |
* @param source the robot whose BOM is on |
546 | 668 |
**/ |
547 |
void wl_token_bom_on_receive(int source) |
|
669 |
static void wl_token_bom_on_receive(int source)
|
|
548 | 670 |
{ |
549 | 671 |
WL_DEBUG_PRINT("Robot "); |
550 | 672 |
WL_DEBUG_PRINT_INT(source); |
... | ... | |
563 | 685 |
* |
564 | 686 |
* If there is a pending request for the token, this is processed first. |
565 | 687 |
**/ |
566 |
void wl_token_get_token() |
|
688 |
static void wl_token_get_token()
|
|
567 | 689 |
{ |
568 | 690 |
WL_DEBUG_PRINT("We have the token.\r\n"); |
569 | 691 |
if (ringState == ACCEPTED) |
570 | 692 |
{ |
571 |
sensor_matrix_set_in_ring(sensorMatrix, |
|
572 |
wl_get_xbee_id(), 1); |
|
693 |
sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1); |
|
573 | 694 |
WL_DEBUG_PRINT("Now a member of the token ring.\r\n"); |
574 | 695 |
ringState = MEMBER; |
575 | 696 |
joinDelay = -1; |
... | ... | |
586 | 707 |
} |
587 | 708 |
|
588 | 709 |
WL_DEBUG_PRINT("Our BOM has been flashed.\r\n"); |
589 |
wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON, |
|
590 |
NULL, 0, 0); |
|
710 |
wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON, NULL, 0, 0); |
|
591 | 711 |
|
592 | 712 |
bom_on_function(); |
593 | 713 |
#ifdef ROBOT |
... | ... | |
612 | 732 |
* |
613 | 733 |
* @param source the robot who requested to join |
614 | 734 |
**/ |
615 |
void wl_token_join_receive(int source) |
|
735 |
static void wl_token_join_receive(int source)
|
|
616 | 736 |
{ |
617 | 737 |
WL_DEBUG_PRINT("Received joining request from robot "); |
618 | 738 |
WL_DEBUG_PRINT_INT(source); |
... | ... | |
664 | 784 |
* |
665 | 785 |
* @param source the robot who accepted us |
666 | 786 |
**/ |
667 |
void wl_token_join_accept_receive(int source) |
|
787 |
static void wl_token_join_accept_receive(int source)
|
|
668 | 788 |
{ |
669 | 789 |
WL_DEBUG_PRINT("Accepted into the token ring by robot "); |
670 | 790 |
WL_DEBUG_PRINT_INT(source); |
... | ... | |
676 | 796 |
//add ourselves to the token ring |
677 | 797 |
sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1); |
678 | 798 |
} |
679 |
|
|
680 |
/** |
|
681 |
* Returns the number of robots in the token ring. |
|
682 |
* |
|
683 |
* @return the number of robots in the token ring |
|
684 |
**/ |
|
685 |
int wl_token_get_robots_in_ring(void) |
|
686 |
{ |
|
687 |
return sensor_matrix_get_joined(sensorMatrix); |
|
688 |
} |
|
689 |
|
|
690 |
/** |
|
691 |
* Returns true if the specified robot is in the token ring, false |
|
692 |
* otherwise. |
|
693 |
* |
|
694 |
* @param robot the robot to check for whether it is in the token ring |
|
695 |
* @return nonzero if the robot is in the token ring, zero otherwise |
|
696 |
**/ |
|
697 |
int wl_token_is_robot_in_ring(int robot) |
|
698 |
{ |
|
699 |
return sensor_matrix_get_in_ring(sensorMatrix, robot); |
|
700 |
} |
|
701 |
|
|
702 |
/** |
|
703 |
* Begins iterating through the robots in the token ring. |
|
704 |
* |
|
705 |
* @see wl_token_iterator_has_next, wl_token_iterator_next |
|
706 |
**/ |
|
707 |
void wl_token_iterator_begin(void) |
|
708 |
{ |
|
709 |
int i = 0; |
|
710 |
|
|
711 |
while (!sensor_matrix_get_in_ring(sensorMatrix, i) && i < sensor_matrix_get_size(sensorMatrix)) { |
|
712 |
i++; |
|
713 |
} |
|
714 |
|
|
715 |
if (i == sensor_matrix_get_size(sensorMatrix)) { |
|
716 |
i = -1; |
|
717 |
} |
|
718 |
|
|
719 |
iteratorCount = i; |
|
720 |
} |
|
721 |
|
|
722 |
/** |
|
723 |
* Returns true if there are more robots in the token ring |
|
724 |
* to iterate through, and false otherwise. |
|
725 |
* |
|
726 |
* @return nonzero if there are more robots to iterate through, |
|
727 |
* zero otherwise |
|
728 |
* |
|
729 |
* @see wl_token_iterator_begin, wl_token_iterator_next |
|
730 |
**/ |
|
731 |
int wl_token_iterator_has_next(void) |
|
732 |
{ |
|
733 |
return iteratorCount != -1; |
|
734 |
} |
|
735 |
|
|
736 |
/** |
|
737 |
* Returns the next robot ID in the token ring. |
|
738 |
* |
|
739 |
* @return the next robot ID in the token ring, or -1 if none exists |
|
740 |
* |
|
741 |
* @see wl_token_iterator_begin, wl_token_iterator_has_next |
|
742 |
**/ |
|
743 |
int wl_token_iterator_next(void) |
|
744 |
{ |
|
745 |
int result = iteratorCount; |
|
746 |
if (result < 0) { |
|
747 |
return result; |
|
748 |
} |
|
749 |
|
|
750 |
iteratorCount++; |
|
751 |
while (!sensor_matrix_get_in_ring(sensorMatrix, iteratorCount) |
|
752 |
&& iteratorCount < sensor_matrix_get_size(sensorMatrix)) { |
|
753 |
iteratorCount++; |
|
754 |
} |
|
755 |
|
|
756 |
if (iteratorCount == sensor_matrix_get_size(sensorMatrix)) { |
|
757 |
iteratorCount = -1; |
|
758 |
} |
|
759 |
|
|
760 |
return result; |
|
761 |
} |
|
762 |
|
|
763 |
/** |
|
764 |
* Returns the number of robots currently in the token ring. |
|
765 |
* |
|
766 |
* @return the number of robots in the token ring |
|
767 |
**/ |
|
768 |
int wl_token_get_num_robots(void) |
|
769 |
{ |
|
770 |
return sensor_matrix_get_joined(sensorMatrix); |
|
771 |
} |
|
772 |
|
|
773 |
/** |
|
774 |
* Returns the number of robots in the sensor matrix. |
|
775 |
* |
|
776 |
* @return the number of robots in the sensor matrix |
|
777 |
**/ |
|
778 |
int wl_token_get_matrix_size(void) |
|
779 |
{ |
|
780 |
return sensor_matrix_get_size(sensorMatrix); |
|
781 |
} |
|
782 |
|
Also available in: Unified diff