Project

General

Profile

Revision 736

Major changes to wireless, but still more work to do.

View differences:

wl_token_ring.c
61 61
#define LEAVING 4
62 62

  
63 63
/*Frame Types*/
64
#define TOKEN_JOIN_ACCEPT_FRAME 1
64
#define TOKEN_JOIN_ACCEPT_FRAME	1
65
#define WL_TOKEN_PASS_FRAME	2
65 66

  
66 67
/*Function Prototypes*/
67 68

  
......
77 78
static void wl_token_get_token(void);
78 79

  
79 80
/*Packet Handling Routines*/
80
static void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength);
81
static void wl_token_pass_receive(int source);
82
static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength);
81 83
static void wl_token_bom_on_receive(int source);
82 84
static void wl_token_join_receive(int source);
83 85
static void wl_token_join_accept_receive(int source);
84 86

  
85 87
/*Global Variables*/
86 88

  
87
//the sensor matrix
88
static SensorMatrix* sensorMatrix;
89

  
90 89
//the robot we are waiting to say it has received the token. -1 if unspecified
91 90
static int wl_token_next_robot = -1;
92 91

  
......
108 107
// the amount of time a robot has had its BOM on for
109 108
static int bom_on_count = 0;
110 109

  
110
#ifndef ROBOT
111 111
static void do_nothing(void) {}
112 112
static int get_nothing(void) {return -1;}
113
#endif
113 114

  
114 115
#ifdef ROBOT
115 116
#ifndef FIREFLY
......
178 179
		return -1;
179 180
	}
180 181

  
181
	sensorMatrix = sensor_matrix_create();
182
	sensor_matrix_create();
182 183
	//add ourselves to the sensor matrix
183
	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
184
	sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
184 185

  
185 186
	wl_register_packet_group(&wl_token_ring_handler);
186 187

  
......
224 225
// it reduces code size or not should be done to be sure.
225 226
static void wl_token_ring_cleanup()
226 227
{
227
	sensor_matrix_destroy(sensorMatrix);
228 228
}
229 229

  
230 230
/**
......
239 239
		//also, declare that person dead, as long as it isn't us
240 240
		if (wl_token_next_robot != wl_get_xbee_id())
241 241
		{
242
			sensor_matrix_set_in_ring(sensorMatrix, wl_token_next_robot, 0);
242
			sensor_matrix_set_in_ring(wl_token_next_robot, 0);
243 243
			WL_DEBUG_PRINT("Robot ");
244 244
			WL_DEBUG_PRINT_INT(wl_token_next_robot);
245 245
			WL_DEBUG_PRINT(" has died.\r\n");
......
256 256
	//we must start our own token ring, no one is responding to us
257 257
	if (joinDelay == 0)
258 258
	{
259
		if (sensor_matrix_get_joined(sensorMatrix) == 0)
259
		if (sensor_matrix_get_joined() == 0)
260 260
		{
261 261
			WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");
262
			sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
262
			sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
263 263
			ringState = MEMBER;
264 264
			//this will make us pass the token to ourself
265 265
			//repeatedly, and other robots when they join
......
314 314
	switch (type)
315 315
	{
316 316
		case WL_TOKEN_PASS:
317
			if (length < 1)
318
			{
319
				WL_DEBUG_PRINT("Malformed Token Pass packet received.\r\n");
320
				return;
321
			}
322
			wl_token_pass_receive(source, packet[0], packet + 1, length - 1);
317
			wl_token_pass_receive(source);
323 318
			break;
319
		case WL_TOKEN_SENSOR_MATRIX:
320
			wl_token_sensor_matrix_receive(source, packet, length);
321
			break;
324 322
		case WL_TOKEN_BOM_ON:
325 323
			//add the robot to the sensor matrix if it is not already there
326 324
			wl_token_bom_on_receive(source);
......
350 348
{
351 349
	if (wl_token_is_robot_in_ring(dest) &&
352 350
			(source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source))) {
353
		return sensor_matrix_get_reading(sensorMatrix, source, dest);
351
		return sensor_matrix_get_reading(source, dest);
354 352
	}
355 353

  
356 354
	return -1;
......
381 379
// it reduces code size or not should be done to be sure.
382 380
int wl_token_get_robots_in_ring(void)
383 381
{
384
	return sensor_matrix_get_joined(sensorMatrix);
382
	return sensor_matrix_get_joined();
385 383
}
386 384

  
387 385
/**
......
395 393
// it reduces code size or not should be done to be sure.
396 394
int wl_token_is_robot_in_ring(int robot)
397 395
{
398
	return sensor_matrix_get_in_ring(sensorMatrix, robot);
396
	return sensor_matrix_get_in_ring(robot);
399 397
}
400 398

  
401 399
/**
......
410 408
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
411 409
  // the overhead of a function call each iteration, call it only once before the loop and store
412 410
  // the value in a variable and check against that variable in the loop condition
413
	while (!sensor_matrix_get_in_ring(sensorMatrix, i) && i < sensor_matrix_get_size(sensorMatrix)) {
411
	while (!sensor_matrix_get_in_ring(i) && i < sensor_matrix_get_size()) {
414 412
		i++;
415 413
	}
416 414

  
417 415
  //TODO: if you do the above comment, then you can compare this to the variable also
418
	if (i == sensor_matrix_get_size(sensorMatrix)) {
416
	if (i == sensor_matrix_get_size()) {
419 417
		i = -1;
420 418
	}
421 419

  
......
457 455
  // the overhead of a function call each iteration, call it only once before the loop and store
458 456
  // the value in a variable and check against that variable in the loop condition
459 457
	iteratorCount++;
460
	while (!sensor_matrix_get_in_ring(sensorMatrix, iteratorCount)
461
		&& iteratorCount < sensor_matrix_get_size(sensorMatrix)) {
458
	while (!sensor_matrix_get_in_ring(iteratorCount)
459
		&& iteratorCount < sensor_matrix_get_size()) {
462 460
		iteratorCount++;
463 461
	}
464 462

  
465 463
  //TODO: if you do the above comment, then you can compare this to the variable also
466
	if (iteratorCount == sensor_matrix_get_size(sensorMatrix)) {
464
	if (iteratorCount == sensor_matrix_get_size()) {
467 465
		iteratorCount = -1;
468 466
	}
469 467

  
......
479 477
// it reduces code size or not should be done to be sure.
480 478
int wl_token_get_num_robots(void)
481 479
{
482
	return sensor_matrix_get_joined(sensorMatrix);
480
	return sensor_matrix_get_joined();
483 481
}
484 482

  
485 483
/**
......
491 489
// it reduces code size or not should be done to be sure.
492 490
int wl_token_get_matrix_size(void)
493 491
{
494
	return sensor_matrix_get_size(sensorMatrix);
492
	return sensor_matrix_get_size();
495 493
}
496 494

  
497 495
/**
498 496
 * This method is called when we receive a token pass packet.
497
 *
498
 * @param source the robot who passed the token to us.
499
 **/
500
static void wl_token_pass_receive(int source)
501
{
502
	WL_DEBUG_PRINT("Received token from ");
503
	WL_DEBUG_PRINT_INT(source);
504
	WL_DEBUG_PRINT(", expected ");
505
	WL_DEBUG_PRINT_INT(wl_token_next_robot);
506
	WL_DEBUG_PRINT(".\n");
507
	// this prevents two tokens from being passed around at a time (second clause is in case we are joining)
508
	if ((source != wl_token_next_robot && wl_get_xbee_id() != wl_token_next_robot) && bom_on_count <= DEATH_DELAY / 2 &&
509
		ringState != ACCEPTED)
510
	{
511
		WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
512
		WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
513
		return;
514
	}
515
	bom_on_count = -1;
516
	deathDelay = -1;
517
	sensor_matrix_set_in_ring(source, 1);
518
	wl_token_get_token();
519
}
520

  
521
/**
522
 * This method is called when we receive a token pass packet.
499 523
 * @param source is the robot it came from
500 524
 * @param nextRobot is the robot the token was passed to
501 525
 * @param sensorData a char with an id followed by a char with the sensor
502 526
 *		reading for that robot, repeated for sensorDataLength bytes
503 527
 * @param sensorDataLength the length in bytes of sensorData
504 528
 */
505
static void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength)
529
static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength)
506 530
{
507 531
	int i, j;
532
	char nextRobot;
508 533

  
509
	// this prevents two tokens from being passed around at a time (second clause is in case we are joining)
510
	if (source != wl_token_next_robot && bom_on_count <= DEATH_DELAY / 2 &&
511
		ringState != ACCEPTED)
512
	{
513
		WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
514
		WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
515
		return;
516
	}
517

  
518 534
	bom_on_count = -1;
519 535
	deathDelay = -1;
520
	WL_DEBUG_PRINT("Received the token from robot");
521
	WL_DEBUG_PRINT_INT(source);
522
	WL_DEBUG_PRINT(", next robot is ");
523
	WL_DEBUG_PRINT_INT((int)nextRobot);
524
	WL_DEBUG_PRINT(" \r\n");
525
	sensor_matrix_set_in_ring(sensorMatrix, source, 1);
536
	sensor_matrix_set_in_ring(source, 1);
526 537

  
527 538
	//with this packet, we are passed the id of the next robot in the ring
528 539
	//and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)
......
531 542
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
532 543
  // the overhead of a function call each iteration, call it only once before the loop and store
533 544
  // the value in a variable and check against that variable in the loop condition
534
	for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
545
	for (i = 0; i < sensor_matrix_get_size(); i++)
535 546
	{
536 547
		if (i == source) {
537 548
			continue;
......
546 557
				accepted = -1;
547 558
				WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
548 559
			}
549
			sensor_matrix_set_reading(sensorMatrix, source, i,
560
			sensor_matrix_set_reading(source, i,
550 561
						sensorData[2 * j + 1]);
551
			sensor_matrix_set_in_ring(sensorMatrix, i, 1);
562
			if (!sensor_matrix_get_in_ring(i))
563
			{
564
				WL_DEBUG_PRINT("Robot ");
565
				WL_DEBUG_PRINT_INT(i);
566
				WL_DEBUG_PRINT(" has been added to the sensor matrix of robot ");
567
				WL_DEBUG_PRINT_INT(wl_get_xbee_id());
568
				WL_DEBUG_PRINT(" due to a packet from robot ");
569
				WL_DEBUG_PRINT_INT(source);
570
				WL_DEBUG_PRINT(".\r\n");
571
			}
572
			sensor_matrix_set_in_ring(i, 1);
552 573
			j++;
553 574
		}
554 575
		else
555 576
		{
556
			if (sensor_matrix_get_in_ring(sensorMatrix, i))
577
			if (sensor_matrix_get_in_ring(i))
557 578
			{
558 579
				WL_DEBUG_PRINT("Robot ");
559 580
				WL_DEBUG_PRINT_INT(i);
......
562 583
				WL_DEBUG_PRINT(" due to a packet from robot ");
563 584
				WL_DEBUG_PRINT_INT(source);
564 585
				WL_DEBUG_PRINT(".\r\n");
565
				sensor_matrix_set_in_ring(sensorMatrix, i, 0);
586
				sensor_matrix_set_in_ring(i, 0);
566 587
			}
567 588

  
568 589
			if (i == wl_get_xbee_id() && ringState == MEMBER)
......
577 598
			//the person who accepted us is dead... let's ask again
578 599
			if (i == acceptor)
579 600
			{
580
				sensor_matrix_set_in_ring(sensorMatrix,
581
						wl_get_xbee_id(), 1);
601
				sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
582 602
				ringState = NONMEMBER;
583 603
				acceptor = -1;
584 604
				wl_token_ring_join();
585 605
			}
586 606
		}
587 607
	}
608
	
609
	// get the next robot in the token ring
610
	i = source + 1;
611
	while (1)
612
	{
613
		if (i == sensor_matrix_get_size()) {
614
			i = 0;
615
		}
588 616

  
589
	wl_token_next_robot = nextRobot;
617
		if (sensor_matrix_get_in_ring(i) || i == source)
618
		{
619
			nextRobot = (char)i;
620
			break;
621
		}
590 622

  
623
		i++;
624
	}
625

  
626
	if (nextRobot != wl_get_xbee_id())
627
		wl_token_next_robot = nextRobot;
628

  
591 629
	deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;
592 630

  
593
	//we have the token
594
	if (wl_token_next_robot == wl_get_xbee_id()) {
595
		wl_token_get_token();
596
	}
631
	if (sensor_matrix_get_joined() == 0 && ringState == JOINING)
632
		wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME);
597 633
}
598 634

  
599 635
/**
......
615 651
    // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
616 652
    // the overhead of a function call each iteration, call it only once before the loop and store
617 653
    // the value in a variable and check against that variable in the loop condition
618
		if (curr == sensor_matrix_get_size(sensorMatrix))
654
		if (curr == sensor_matrix_get_size())
619 655
			curr = 0;
620 656
		if (curr == robot2)
621 657
			break;
622
		if (sensor_matrix_get_in_ring(sensorMatrix, curr))
658
		if (sensor_matrix_get_in_ring(curr))
623 659
			count++;
624 660
		curr++;
625 661
	}
......
631 667
 **/
632 668
static int wl_token_pass_token()
633 669
{
634
	char nextRobot;
670
	char nextRobot = 0xFF;
635 671
	int i = wl_get_xbee_id() + 1;
672
	char buf[2 * sensor_matrix_get_size()];
636 673
	if (accepted == -1)
637 674
	{
638 675
		while (1)
......
641 678
      // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
642 679
      // the overhead of a function call each iteration, call it only once before the loop and store
643 680
      // the value in a variable and check against that variable in the loop condition
644
			if (i == sensor_matrix_get_size(sensorMatrix)) {
681
			if (i == sensor_matrix_get_size()) {
645 682
				i = 0;
646 683
			}
647 684

  
648
			if (sensor_matrix_get_in_ring(sensorMatrix, i))
685
			if (sensor_matrix_get_in_ring(i))
649 686
			{
650 687
				nextRobot = (char)i;
651 688
				break;
......
658 695
	{
659 696
		WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");
660 697
		//add a new robot to the token ring
661
		sensor_matrix_set_in_ring(sensorMatrix, accepted, 1);
698
		sensor_matrix_set_in_ring(accepted, 1);
662 699
		nextRobot = accepted;
663 700
		accepted = -1;
664 701
	}
665 702

  
666
	//we don't include ourself
667
	int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
668
	char* buf = (char*)malloc(packetSize * sizeof(char));
669
	if (!buf)
670
	{
671
		WL_DEBUG_PRINT_INT(packetSize);
672
		WL_DEBUG_PRINT("Out of memory - pass token.\r\n");
673
		free(buf);
674
		return -1;
675
	}
676
	buf[0] = nextRobot;
677

  
678 703
	int j = 0;
679 704
  //TODO: the compiler may or may not optimize this such that my comment is useless:
680 705
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
681 706
  // the overhead of a function call each iteration, call it only once before the loop and store
682 707
  // the value in a variable and check against that variable in the loop condition
683
	for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++) {
684
		if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
708
	for (i = 0; i < sensor_matrix_get_size(); i++) {
709
		if (sensor_matrix_get_in_ring(i) && i != wl_get_xbee_id())
685 710
		{
686
			buf[2*j + 1] = i;
687
			buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);
711
			buf[2*j] = i;
712
			buf[2*j + 1] = sensor_matrix_get_reading(wl_get_xbee_id(), i);
688 713
			j++;
689 714
		}
690 715
	}
691 716

  
717
	int packetSize = 2 * j * sizeof(char);
692 718
	WL_DEBUG_PRINT("Passing the token to robot ");
693
	WL_DEBUG_PRINT_INT(buf[0]);
719
	WL_DEBUG_PRINT_INT(nextRobot);
694 720
	WL_DEBUG_PRINT(".\r\n");
695
	if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, buf, packetSize, 3) != 0) {
696
		free(buf);
721
	if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0)
697 722
		return -1;
698
	}
723
	if (wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME))
724
		return -1;
699 725

  
700 726
	wl_token_next_robot = nextRobot;
701 727
	deathDelay = DEATH_DELAY;
702
	free(buf);
703 728

  
704 729
	return 0;
705 730
}
......
719 744

  
720 745
	bom_on_count = 0;
721 746

  
722
	sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(),
747
	sensor_matrix_set_reading(wl_get_xbee_id(),
723 748
		source, get_max_bom_function());
724 749
}
725 750

  
......
735 760
	WL_DEBUG_PRINT("We have the token.\r\n");
736 761
	if (ringState == ACCEPTED)
737 762
	{
738
		sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
763
		sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
739 764
		WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
740 765
		ringState = MEMBER;
741 766
		joinDelay = -1;
......
743 768

  
744 769
	if (ringState == LEAVING || ringState == NONMEMBER)
745 770
	{
746
		sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
771
		sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
747 772
		if (ringState == NONMEMBER)
748 773
		{
749 774
			WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
......
760 785
	#endif
761 786
	bom_off_function();
762 787

  
763
	if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
788
	if (!sensor_matrix_get_in_ring(wl_get_xbee_id()))
764 789
	{
765 790
		WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
766 791
		return;
......
798 823
	while (1)
799 824
	{
800 825
		if (i < 0)
801
			i = sensor_matrix_get_size(sensorMatrix) - 1;
826
			i = sensor_matrix_get_size() - 1;
802 827

  
803 828
		//we must send a join acceptance
804 829
		if (i == wl_get_xbee_id())
805 830
			break;
806 831

  
807 832
		//another robot will handle it
808
		if (sensor_matrix_get_in_ring(sensorMatrix, i))
833
		if (sensor_matrix_get_in_ring(i))
809 834
			return;
810 835

  
811 836
		i--;
......
820 845
	WL_DEBUG_PRINT(" into the token ring.\r\n");
821 846

  
822 847
	// the token ring has not started yet
823
	if (sensor_matrix_get_joined(sensorMatrix) == 1)
848
	if (sensor_matrix_get_joined() == 1)
824 849
		wl_token_pass_token();
825 850
}
826 851

  
......
841 866
	acceptor = source;
842 867

  
843 868
	//add ourselves to the token ring
844
	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
869
	sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
845 870
}

Also available in: Unified diff