500 |
500 |
static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength)
|
501 |
501 |
{
|
502 |
502 |
int i, j;
|
503 |
|
// get the next robot in the token ring
|
504 |
503 |
char nextRobot;
|
505 |
|
i = source + 1;
|
506 |
|
while (1)
|
507 |
|
{
|
508 |
|
if (i == sensor_matrix_get_size()) {
|
509 |
|
i = 0;
|
510 |
|
}
|
511 |
504 |
|
512 |
|
if (sensor_matrix_get_in_ring(i))
|
513 |
|
{
|
514 |
|
nextRobot = (char)i;
|
515 |
|
break;
|
516 |
|
}
|
517 |
|
|
518 |
|
i++;
|
519 |
|
}
|
520 |
|
|
521 |
505 |
bom_on_count = -1;
|
522 |
506 |
deathDelay = -1;
|
523 |
507 |
sensor_matrix_set_in_ring(source, 1);
|
... | ... | |
588 |
572 |
}
|
589 |
573 |
}
|
590 |
574 |
}
|
|
575 |
|
|
576 |
// get the next robot in the token ring
|
|
577 |
i = source + 1;
|
|
578 |
while (1)
|
|
579 |
{
|
|
580 |
if (i == sensor_matrix_get_size()) {
|
|
581 |
i = 0;
|
|
582 |
}
|
591 |
583 |
|
592 |
|
wl_token_next_robot = nextRobot;
|
|
584 |
if (sensor_matrix_get_in_ring(i) || i == source)
|
|
585 |
{
|
|
586 |
nextRobot = (char)i;
|
|
587 |
break;
|
|
588 |
}
|
593 |
589 |
|
|
590 |
i++;
|
|
591 |
}
|
|
592 |
|
|
593 |
if (nextRobot != wl_get_xbee_id())
|
|
594 |
wl_token_next_robot = nextRobot;
|
|
595 |
|
594 |
596 |
deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;
|
595 |
597 |
|
596 |
598 |
if (sensor_matrix_get_joined() == 0 && ringState == JOINING)
|
... | ... | |
631 |
633 |
char nextRobot = 0xFF;
|
632 |
634 |
int i = wl_get_xbee_id() + 1;
|
633 |
635 |
char buf[2 * sensor_matrix_get_size()];
|
634 |
|
WL_DEBUG_PRINT("PASSING\n");
|
635 |
636 |
if (accepted == -1)
|
636 |
637 |
{
|
637 |
638 |
while (1)
|
... | ... | |
657 |
658 |
nextRobot = accepted;
|
658 |
659 |
accepted = -1;
|
659 |
660 |
}
|
660 |
|
WL_DEBUG_PRINT("2\n");
|
661 |
661 |
|
662 |
662 |
int j = 0;
|
663 |
663 |
for (i = 0; i < sensor_matrix_get_size(); i++) {
|
... | ... | |
669 |
669 |
}
|
670 |
670 |
}
|
671 |
671 |
|
672 |
|
int packetSize = 2 * sensor_matrix_get_size() * sizeof(char);
|
|
672 |
int packetSize = 2 * j * sizeof(char);
|
673 |
673 |
WL_DEBUG_PRINT("Passing the token to robot ");
|
674 |
674 |
WL_DEBUG_PRINT_INT(nextRobot);
|
675 |
675 |
WL_DEBUG_PRINT(".\r\n");
|
676 |
|
if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0) {
|
677 |
|
free(buf);
|
|
676 |
if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0)
|
678 |
677 |
return -1;
|
679 |
|
}
|
680 |
678 |
if (wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME))
|
681 |
|
{
|
682 |
|
free(buf);
|
683 |
679 |
return -1;
|
684 |
|
}
|
685 |
680 |
|
686 |
681 |
wl_token_next_robot = nextRobot;
|
687 |
682 |
deathDelay = DEATH_DELAY;
|
688 |
|
free(buf);
|
689 |
683 |
|
690 |
684 |
return 0;
|
691 |
685 |
}
|