59 |
59 |
#define LEAVING 4
|
60 |
60 |
|
61 |
61 |
/*Frame Types*/
|
62 |
|
#define TOKEN_JOIN_ACCEPT_FRAME 1
|
|
62 |
#define TOKEN_JOIN_ACCEPT_FRAME 1
|
|
63 |
#define WL_TOKEN_PASS_FRAME 2
|
63 |
64 |
|
64 |
65 |
/*Function Prototypes*/
|
65 |
66 |
|
... | ... | |
75 |
76 |
static void wl_token_get_token(void);
|
76 |
77 |
|
77 |
78 |
/*Packet Handling Routines*/
|
78 |
|
static void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength);
|
|
79 |
static void wl_token_pass_receive(int source);
|
|
80 |
static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength);
|
79 |
81 |
static void wl_token_bom_on_receive(int source);
|
80 |
82 |
static void wl_token_join_receive(int source);
|
81 |
83 |
static void wl_token_join_accept_receive(int source);
|
... | ... | |
106 |
108 |
// the amount of time a robot has had its BOM on for
|
107 |
109 |
static int bom_on_count = 0;
|
108 |
110 |
|
|
111 |
#ifndef ROBOT
|
109 |
112 |
static void do_nothing(void) {}
|
110 |
113 |
static int get_nothing(void) {return -1;}
|
|
114 |
#endif
|
111 |
115 |
|
112 |
116 |
#ifdef ROBOT
|
113 |
117 |
#ifndef FIREFLY
|
... | ... | |
306 |
310 |
switch (type)
|
307 |
311 |
{
|
308 |
312 |
case WL_TOKEN_PASS:
|
|
313 |
wl_token_pass_receive(source);
|
|
314 |
break;
|
|
315 |
case WL_TOKEN_SENSOR_MATRIX:
|
309 |
316 |
if (length < 1)
|
310 |
317 |
{
|
311 |
318 |
WL_DEBUG_PRINT("Malformed Token Pass packet received.\r\n");
|
312 |
319 |
return;
|
313 |
320 |
}
|
314 |
|
wl_token_pass_receive(source, packet[0], packet + 1, length - 1);
|
315 |
|
break;
|
|
321 |
wl_token_sensor_matrix_receive(source, packet, length);
|
316 |
322 |
case WL_TOKEN_BOM_ON:
|
317 |
323 |
//add the robot to the sensor matrix if it is not already there
|
318 |
324 |
wl_token_bom_on_receive(source);
|
... | ... | |
467 |
473 |
|
468 |
474 |
/**
|
469 |
475 |
* This method is called when we receive a token pass packet.
|
|
476 |
*
|
|
477 |
* @param source the robot who passed the token to us.
|
|
478 |
**/
|
|
479 |
static void wl_token_pass_receive(int source)
|
|
480 |
{
|
|
481 |
// this prevents two tokens from being passed around at a time (second clause is in case we are joining)
|
|
482 |
if (source != wl_token_next_robot && bom_on_count <= DEATH_DELAY / 2 &&
|
|
483 |
ringState != ACCEPTED)
|
|
484 |
{
|
|
485 |
WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
|
|
486 |
WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
|
|
487 |
return;
|
|
488 |
}
|
|
489 |
bom_on_count = -1;
|
|
490 |
deathDelay = -1;
|
|
491 |
sensor_matrix_set_in_ring(sensorMatrix, source, 1);
|
|
492 |
wl_token_get_token();
|
|
493 |
}
|
|
494 |
|
|
495 |
/**
|
|
496 |
* This method is called when we receive a token pass packet.
|
470 |
497 |
* @param source is the robot it came from
|
471 |
498 |
* @param nextRobot is the robot the token was passed to
|
472 |
499 |
* @param sensorData a char with an id followed by a char with the sensor
|
473 |
500 |
* reading for that robot, repeated for sensorDataLength bytes
|
474 |
501 |
* @param sensorDataLength the length in bytes of sensorData
|
475 |
502 |
*/
|
476 |
|
static void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength)
|
|
503 |
static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength)
|
477 |
504 |
{
|
478 |
505 |
int i, j;
|
|
506 |
// get the next robot in the token ring
|
|
507 |
char nextRobot;
|
|
508 |
i = source + 1;
|
|
509 |
while (1)
|
|
510 |
{
|
|
511 |
if (i == sensor_matrix_get_size(sensorMatrix)) {
|
|
512 |
i = 0;
|
|
513 |
}
|
479 |
514 |
|
480 |
|
// 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;
|
|
515 |
if (sensor_matrix_get_in_ring(sensorMatrix, i))
|
|
516 |
{
|
|
517 |
nextRobot = (char)i;
|
|
518 |
break;
|
|
519 |
}
|
|
520 |
|
|
521 |
i++;
|
487 |
522 |
}
|
488 |
523 |
|
489 |
524 |
bom_on_count = -1;
|
490 |
525 |
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 |
526 |
sensor_matrix_set_in_ring(sensorMatrix, source, 1);
|
497 |
527 |
|
498 |
528 |
//with this packet, we are passed the id of the next robot in the ring
|
... | ... | |
557 |
587 |
|
558 |
588 |
deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;
|
559 |
589 |
|
560 |
|
//we have the token
|
561 |
|
if (wl_token_next_robot == wl_get_xbee_id()) {
|
562 |
|
wl_token_get_token();
|
563 |
|
}
|
|
590 |
if (sensor_matrix_get_size(sensorMatrix) == 0 && ringState == JOINING)
|
|
591 |
wl_send_robot_to_robot_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME);
|
564 |
592 |
}
|
565 |
593 |
|
566 |
594 |
/**
|
... | ... | |
594 |
622 |
**/
|
595 |
623 |
static int wl_token_pass_token()
|
596 |
624 |
{
|
597 |
|
char nextRobot;
|
|
625 |
char nextRobot = 0xFF;
|
598 |
626 |
int i = wl_get_xbee_id() + 1;
|
599 |
627 |
if (accepted == -1)
|
600 |
628 |
{
|
... | ... | |
623 |
651 |
}
|
624 |
652 |
|
625 |
653 |
//we don't include ourself
|
626 |
|
int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
|
|
654 |
int packetSize = 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
|
627 |
655 |
char* buf = (char*)malloc(packetSize * sizeof(char));
|
628 |
656 |
if (!buf)
|
629 |
657 |
{
|
... | ... | |
632 |
660 |
free(buf);
|
633 |
661 |
return -1;
|
634 |
662 |
}
|
635 |
|
buf[0] = nextRobot;
|
636 |
663 |
|
637 |
664 |
int j = 0;
|
638 |
665 |
for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++) {
|
639 |
666 |
if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
|
640 |
667 |
{
|
641 |
|
buf[2*j + 1] = i;
|
642 |
|
buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);
|
|
668 |
buf[2*j] = i;
|
|
669 |
buf[2*j + 1] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);
|
643 |
670 |
j++;
|
644 |
671 |
}
|
645 |
672 |
}
|
646 |
673 |
|
647 |
674 |
WL_DEBUG_PRINT("Passing the token to robot ");
|
648 |
|
WL_DEBUG_PRINT_INT(buf[0]);
|
|
675 |
WL_DEBUG_PRINT_INT(nextRobot);
|
649 |
676 |
WL_DEBUG_PRINT(".\r\n");
|
650 |
|
if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, buf, packetSize, 3) != 0) {
|
|
677 |
if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0) {
|
651 |
678 |
free(buf);
|
652 |
679 |
return -1;
|
653 |
680 |
}
|
|
681 |
if (wl_send_robot_to_robot_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME))
|
|
682 |
{
|
|
683 |
free(buf);
|
|
684 |
return -1;
|
|
685 |
}
|
654 |
686 |
|
655 |
687 |
wl_token_next_robot = nextRobot;
|
656 |
688 |
deathDelay = DEATH_DELAY;
|