Statistics
| Revision:

root / demos / joystick / lib / src / libwireless / wl_token_ring.c @ 1736

History | View | Annotate | Download (23.2 KB)

1
/**
2
 * Copyright (c) 2007 Colony Project
3
 *
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 *
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 *
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

    
26
/**
27
 * @file wl_token_ring.c
28
 * @brief Token Ring Implementation
29
 *
30
 * Implementation of the token ring packet group.
31
 *
32
 * @author Brian Coltin, Colony Project, CMU Robotics Club
33
 **/
34

    
35
#include <wl_token_ring.h>
36

    
37
#include <stdlib.h>
38

    
39
#include <wl_defs.h>
40
#include <wireless.h>
41
#include <sensor_matrix.h>
42

    
43
#ifdef ROBOT
44
#ifndef FIREFLY
45
#include <bom.h>
46
#endif
47
#include <time.h>
48
#endif
49

    
50

    
51
//#define DEFAULT_SENSOR_MATRIX_SIZE 20
52

    
53
/*Ring States*/
54

    
55
#define NONMEMBER 0
56
#define MEMBER 1
57
#define JOINING 2
58
#define ACCEPTED 3
59
#define LEAVING 4
60

    
61
/*Frame Types*/
62
#define TOKEN_JOIN_ACCEPT_FRAME        1
63
#define WL_TOKEN_PASS_FRAME        2
64

    
65
/*Function Prototypes*/
66

    
67
/*Wireless Library Prototypes*/
68
static void wl_token_ring_timeout_handler(void);
69
static void wl_token_ring_response_handler(int frame, int received);
70
static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length);
71
static void wl_token_ring_cleanup(void);
72

    
73
/*Helper Functions*/
74
static int wl_token_pass_token(void);
75
static int get_token_distance(int robot1, int robot2);
76
static void wl_token_get_token(void);
77

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

    
85
/*Global Variables*/
86

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

    
90
//true if the robot should be in the token ring, 0 otherwise
91
static int ringState = NONMEMBER;
92
//the id of the robot who accepted us into the token ring, only used in ACCEPTED state
93
static int acceptor = -1;
94
//id of the robot we are accepting
95
static int accepted = -1;
96

    
97
//the counter for when we assume a robot is dead
98
static int deathDelay = -1;
99
//the counter for joining, before we form our own token ring
100
static int joinDelay = -1;
101

    
102
//current robot to check in the iterator
103
static int iteratorCount = 0;
104

    
105
// the amount of time a robot has had its BOM on for
106
static int bom_on_count = 0;
107

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

    
113
#ifdef ROBOT
114
#ifndef FIREFLY
115
static void (*bom_on_function) (void) = bom_on;
116
static void (*bom_off_function) (void) = bom_off;
117
static int (*get_max_bom_function) (void) = get_max_bom;
118
#else
119
static void (*bom_on_function) (void) = do_nothing;
120
static void (*bom_off_function) (void) = do_nothing;
121
static int (*get_max_bom_function) (void) = get_nothing;
122
#endif
123
#else
124
static void (*bom_on_function) (void) = do_nothing;
125
static void (*bom_off_function) (void) = do_nothing;
126
static int (*get_max_bom_function) (void) = get_nothing;
127
#endif
128

    
129
static PacketGroupHandler wl_token_ring_handler =
130
        {WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
131
                wl_token_ring_response_handler, wl_token_ring_receive_handler,
132
                wl_token_ring_cleanup};
133

    
134
/**
135
 * Causes the robot to join an existing token ring, or create one
136
 * if no token ring exists. The token ring uses global and robot to robot
137
 * packets, and does not rely on any PAN.
138
 **/
139
int wl_token_ring_join()
140
{
141
        WL_DEBUG_PRINT("Joining the token ring.\r\n");
142

    
143
        ringState = JOINING;
144
        joinDelay = DEATH_DELAY * 2;
145
        if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN, NULL, 0, 0) != 0) {
146
                return -1;
147
        }
148

    
149
        return 0;
150
}
151

    
152
/**
153
 * Causes the robot to leave the token ring. The robot stops
154
 * alerting others of its location, but continues storing the
155
 * locations of other robots.
156
 **/
157
void wl_token_ring_leave()
158
{
159
        ringState = LEAVING;
160
}
161

    
162
/**
163
 * Initialize the token ring packet group and register it with the
164
 * wireless library. The robot will not join a token ring.
165
 **/
166
int wl_token_ring_register()
167
{
168
        if (wl_get_xbee_id() > 0xFF)
169
        {
170
                //Note: if this becomes an issue (unlikely), we could limit sensor information
171
                //to half a byte and use 12 bits for the id
172
                WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");
173
                WL_DEBUG_PRINT_INT(wl_get_xbee_id());
174
                WL_DEBUG_PRINT(".\r\n");
175
                return -1;
176
        }
177

    
178
        sensor_matrix_create();
179
        //add ourselves to the sensor matrix
180
        sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
181

    
182
        wl_register_packet_group(&wl_token_ring_handler);
183

    
184
        return 0;
185
}
186

    
187
/**
188
 * Removes the packet group from the wireless library.
189
 **/
190
void wl_token_ring_unregister()
191
{
192
        wl_unregister_packet_group(&wl_token_ring_handler);
193
}
194

    
195
/**
196
 * Sets the functions that are called when the BOM ought to be
197
 * turned on or off. This could be used for things such as
198
 * charging stations, which have multiple BOMs.
199
 *
200
 * @param on_function the function to be called when the BOM
201
 * should be turned on
202
 * @param off_function the function to be called when the BOM
203
 * should be turned off
204
 * @param max_bom_function the function to be called when a
205
 * measurement of the maximum BOM reading is needed.
206
 **/
207
void wl_token_ring_set_bom_functions(void (*on_function) (void),
208
        void (*off_function) (void), int (*max_bom_function) (void))
209
{
210
        bom_on_function = on_function;
211
        bom_off_function = off_function;
212
        get_max_bom_function = max_bom_function;
213
}
214

    
215
/**
216
 * Called to cleanup the token ring packet group.
217
 **/
218
static void wl_token_ring_cleanup()
219
{
220
}
221

    
222
/**
223
 * Called approximately every quarter second by the wireless library.
224
 **/
225
static void wl_token_ring_timeout_handler()
226
{
227
        //someone is not responding, assume they are dead
228
        if (deathDelay == 0)
229
        {
230
                //pass the token to the next robot if we think someone has died
231
                //also, declare that person dead, as long as it isn't us
232
                if (wl_token_next_robot != wl_get_xbee_id())
233
                {
234
                        sensor_matrix_set_in_ring(wl_token_next_robot, 0);
235
                        WL_DEBUG_PRINT("Robot ");
236
                        WL_DEBUG_PRINT_INT(wl_token_next_robot);
237
                        WL_DEBUG_PRINT(" has died.\r\n");
238
                        wl_token_next_robot = -1;
239
                        deathDelay = DEATH_DELAY;
240
                }
241

    
242
                // we may have been dropped from the ring when this is received
243
                if (ringState == MEMBER) {
244
                        wl_token_pass_token();
245
                }
246
        }
247

    
248
        //we must start our own token ring, no one is responding to us
249
        if (joinDelay == 0)
250
        {
251
                if (sensor_matrix_get_joined() == 0)
252
                {
253
                        WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");
254
                        sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
255
                        ringState = MEMBER;
256
                        //this will make us pass the token to ourself
257
                        //repeatedly, and other robots when they join
258
                        deathDelay = DEATH_DELAY;
259
                        wl_token_next_robot = wl_get_xbee_id();
260
                }
261
                else
262
                {
263
                        WL_DEBUG_PRINT("Attempting to join the token ring again.\r\n");
264
                        //attempt to rejoin with a random delay
265
      //TODO: should we use the constant JOIN_DELAY ?
266
                        wl_token_ring_join();
267
                        joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;
268
                }
269
        }
270

    
271
        if (deathDelay >= 0) {
272
                deathDelay--;
273
        }
274

    
275
        if (joinDelay >= 0) {
276
                joinDelay--;
277
        }
278

    
279
        if (bom_on_count >= 0) {
280
                bom_on_count++;
281
        }
282
}
283

    
284
/**
285
 * Called when the XBee tells us if a packet we sent has been received.
286
 *
287
 * @param frame the frame number assigned when the packet was sent
288
 * @param received 1 if the packet was received, 0 otherwise
289
 **/
290
static void wl_token_ring_response_handler(int frame, int received)
291
{
292
        if (!received)
293
        {
294
                WL_DEBUG_PRINT("FAILED.\r\n");
295
        }
296
}
297

    
298
/**
299
 * Called when we recieve a token ring packet.
300
 * @param type the type of the packet
301
 * @param source the id of the robot who sent the packet
302
 * @param packet the data in the packet
303
 * @param length the length of the packet in bytes
304
 **/
305
static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length)
306
{
307
        switch (type)
308
        {
309
                case WL_TOKEN_PASS:
310
                        wl_token_pass_receive(source);
311
                        break;
312
                case WL_TOKEN_SENSOR_MATRIX:
313
                        wl_token_sensor_matrix_receive(source, packet, length);
314
                        break;
315
                case WL_TOKEN_BOM_ON:
316
                        //add the robot to the sensor matrix if it is not already there
317
                        wl_token_bom_on_receive(source);
318
                        break;
319
                case WL_TOKEN_JOIN:
320
                        wl_token_join_receive(source);
321
                        break;
322
                case WL_TOKEN_JOIN_ACCEPT:
323
                        wl_token_join_accept_receive(source);
324
                        break;
325
                default:
326
                        WL_DEBUG_PRINT("Unimplemented token ring packet received.\r\n");
327
                        break;
328
        }
329
}
330

    
331
/**
332
 * Returns the BOM reading robot source has for robot dest.
333
 *
334
 * @param source the robot that made the BOM reading
335
 * @param dest the robot whose relative location is returned
336
 *
337
 * @return a BOM reading from robot source to robot dest,
338
 * in the range 0-15, or -1 if it is unknown
339
 **/
340
int wl_token_get_sensor_reading(int source, int dest)
341
{
342
        if (wl_token_is_robot_in_ring(dest) &&
343
                        (source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source))) {
344
                return sensor_matrix_get_reading(source, dest);
345
        }
346

    
347
        return -1;
348
}
349

    
350
/**
351
 * Returns the BOM reading we have for robot dest.
352
 *
353
 * @param dest the robot whose relative location is returned
354
 *
355
 * @return a BOM reading from us to robot dest, in the range
356
 * 0-15, or -1 if it is unkown
357
 **/
358
int wl_token_get_my_sensor_reading(int dest)
359
{
360
        return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
361
}
362

    
363

    
364
/**
365
 * Returns the number of robots in the token ring.
366
 *
367
 * @return the number of robots in the token ring
368
 **/
369
int wl_token_get_robots_in_ring(void)
370
{
371
        return sensor_matrix_get_joined();
372
}
373

    
374
/**
375
 * Returns true if the specified robot is in the token ring, false
376
 * otherwise.
377
 *
378
 * @param robot the robot to check for whether it is in the token ring
379
 * @return nonzero if the robot is in the token ring, zero otherwise
380
 **/
381
int wl_token_is_robot_in_ring(int robot)
382
{
383
        return sensor_matrix_get_in_ring(robot);
384
}
385

    
386
/**
387
 * Begins iterating through the robots in the token ring.
388
 *
389
 * @see wl_token_iterator_has_next, wl_token_iterator_next
390
 **/
391
void wl_token_iterator_begin(void)
392
{
393
        int i = 0;
394
  //TODO: the compiler may or may not optimize this such that my comment is useless:
395
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
396
  // the overhead of a function call each iteration, call it only once before the loop and store
397
  // the value in a variable and check against that variable in the loop condition
398
        while (!sensor_matrix_get_in_ring(i) && i < sensor_matrix_get_size()) {
399
                i++;
400
        }
401

    
402
  //TODO: if you do the above comment, then you can compare this to the variable also
403
        if (i == sensor_matrix_get_size()) {
404
                i = -1;
405
        }
406

    
407
        iteratorCount = i;
408
}
409

    
410
/**
411
 * Returns true if there are more robots in the token ring
412
 * to iterate through, and false otherwise.
413
 *
414
 * @return nonzero if there are more robots to iterate through,
415
 * zero otherwise
416
 *
417
 * @see wl_token_iterator_begin, wl_token_iterator_next
418
 **/
419
int wl_token_iterator_has_next(void)
420
{
421
        return iteratorCount != -1;
422
}
423

    
424
/**
425
 * Returns the next robot ID in the token ring.
426
 *
427
 * @return the next robot ID in the token ring, or -1 if none exists
428
 *
429
 * @see wl_token_iterator_begin, wl_token_iterator_has_next
430
 **/
431
int wl_token_iterator_next(void)
432
{
433
        int result = iteratorCount;
434
        if (result < 0) {
435
                return result;
436
        }
437

    
438
  //TODO: the compiler may or may not optimize this such that my comment is useless:
439
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
440
  // the overhead of a function call each iteration, call it only once before the loop and store
441
  // the value in a variable and check against that variable in the loop condition
442
        iteratorCount++;
443
        while (!sensor_matrix_get_in_ring(iteratorCount)
444
                && iteratorCount < sensor_matrix_get_size()) {
445
                iteratorCount++;
446
        }
447

    
448
  //TODO: if you do the above comment, then you can compare this to the variable also
449
        if (iteratorCount == sensor_matrix_get_size()) {
450
                iteratorCount = -1;
451
        }
452

    
453
        return result;
454
}
455

    
456
/**
457
 * Returns the number of robots currently in the token ring.
458
 *
459
 * @return the number of robots in the token ring
460
 **/
461
int wl_token_get_num_robots(void)
462
{
463
        return sensor_matrix_get_joined();
464
}
465

    
466
/**
467
 * Returns the number of robots in the sensor matrix.
468
 *
469
 * @return the number of robots in the sensor matrix
470
 **/
471
int wl_token_get_matrix_size(void)
472
{
473
        return sensor_matrix_get_size();
474
}
475

    
476
/**
477
 * This method is called when we receive a token pass packet.
478
 *
479
 * @param source the robot who passed the token to us.
480
 **/
481
static void wl_token_pass_receive(int source)
482
{
483
        WL_DEBUG_PRINT("Received token from ");
484
        WL_DEBUG_PRINT_INT(source);
485
        WL_DEBUG_PRINT(", expected ");
486
        WL_DEBUG_PRINT_INT(wl_token_next_robot);
487
        WL_DEBUG_PRINT(".\n");
488
        // this prevents two tokens from being passed around at a time (second clause is in case we are joining)
489
        if ((source != wl_token_next_robot && wl_get_xbee_id() != wl_token_next_robot) && bom_on_count <= DEATH_DELAY / 2 &&
490
                ringState != ACCEPTED)
491
        {
492
                WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
493
                WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
494
                return;
495
        }
496
        bom_on_count = -1;
497
        deathDelay = -1;
498
        sensor_matrix_set_in_ring(source, 1);
499
        wl_token_get_token();
500
}
501

    
502
/**
503
 * This method is called when we receive a token pass packet.
504
 * @param source is the robot it came from
505
 * @param nextRobot is the robot the token was passed to
506
 * @param sensorData a char with an id followed by a char with the sensor
507
 *                reading for that robot, repeated for sensorDataLength bytes
508
 * @param sensorDataLength the length in bytes of sensorData
509
 */
510
static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength)
511
{
512
        int i, j;
513
        char nextRobot;
514

    
515
        bom_on_count = -1;
516
        deathDelay = -1;
517
        sensor_matrix_set_in_ring(source, 1);
518

    
519
        //with this packet, we are passed the id of the next robot in the ring
520
        //and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)
521
        j = 0;
522
  //TODO: the compiler may or may not optimize this such that my comment is useless:
523
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
524
  // the overhead of a function call each iteration, call it only once before the loop and store
525
  // the value in a variable and check against that variable in the loop condition
526
        for (i = 0; i < sensor_matrix_get_size(); i++)
527
        {
528
                if (i == source) {
529
                        continue;
530
                }
531

    
532
                //set the sensor information we receive
533
                if (j < sensorDataLength / 2 && sensorData[2 * j] == i)
534
                {
535
                        //the robot we were going to accept has already been accepted
536
                        if (accepted == i)
537
                        {
538
                                accepted = -1;
539
                                WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
540
                        }
541
                        sensor_matrix_set_reading(source, i,
542
                                                sensorData[2 * j + 1]);
543
                        if (!sensor_matrix_get_in_ring(i))
544
                        {
545
                                WL_DEBUG_PRINT("Robot ");
546
                                WL_DEBUG_PRINT_INT(i);
547
                                WL_DEBUG_PRINT(" has been added to the sensor matrix of robot ");
548
                                WL_DEBUG_PRINT_INT(wl_get_xbee_id());
549
                                WL_DEBUG_PRINT(" due to a packet from robot ");
550
                                WL_DEBUG_PRINT_INT(source);
551
                                WL_DEBUG_PRINT(".\r\n");
552
                        }
553
                        sensor_matrix_set_in_ring(i, 1);
554
                        j++;
555
                }
556
                else
557
                {
558
                        if (sensor_matrix_get_in_ring(i))
559
                        {
560
                                WL_DEBUG_PRINT("Robot ");
561
                                WL_DEBUG_PRINT_INT(i);
562
                                WL_DEBUG_PRINT(" has been removed from the sensor matrix of robot ");
563
                                WL_DEBUG_PRINT_INT(wl_get_xbee_id());
564
                                WL_DEBUG_PRINT(" due to a packet from robot ");
565
                                WL_DEBUG_PRINT_INT(source);
566
                                WL_DEBUG_PRINT(".\r\n");
567
                                sensor_matrix_set_in_ring(i, 0);
568
                        }
569

    
570
                        if (i == wl_get_xbee_id() && ringState == MEMBER)
571
                        {
572
                                ringState = NONMEMBER;
573
                                wl_token_ring_join();
574

    
575
                                WL_DEBUG_PRINT("We have been removed from the ring ");
576
                                WL_DEBUG_PRINT("and are rejoining.\r\n");
577
                        }
578

    
579
                        //the person who accepted us is dead... let's ask again
580
                        if (i == acceptor)
581
                        {
582
                                sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
583
                                ringState = NONMEMBER;
584
                                acceptor = -1;
585
                                wl_token_ring_join();
586
                        }
587
                }
588
        }
589
        
590
        // get the next robot in the token ring
591
        i = source + 1;
592
        while (1)
593
        {
594
                if (i == sensor_matrix_get_size()) {
595
                        i = 0;
596
                }
597

    
598
                if (sensor_matrix_get_in_ring(i) || i == source)
599
                {
600
                        nextRobot = (char)i;
601
                        break;
602
                }
603

    
604
                i++;
605
        }
606

    
607
        if (nextRobot != wl_get_xbee_id())
608
                wl_token_next_robot = nextRobot;
609

    
610
        deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;
611

    
612
        if (sensor_matrix_get_joined() == 0 && ringState == JOINING)
613
                wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME);
614
}
615

    
616
/**
617
 * Gets the distance in the token ring between two robots.
618
 *
619
 * @param robot1 the first robot
620
 * @param robot2 the second robot
621
 *
622
 * @return the number of passes before the token is expected
623
 * to reach robot2 from robot1
624
 **/
625
static int get_token_distance(int robot1, int robot2)
626
{
627
        int curr = robot1 + 1;
628
        int count = 1;
629
        while (1)
630
        {
631
    //TODO: the compiler may or may not optimize this such that my comment is useless:
632
    // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
633
    // the overhead of a function call each iteration, call it only once before the loop and store
634
    // the value in a variable and check against that variable in the loop condition
635
                if (curr == sensor_matrix_get_size())
636
                        curr = 0;
637
                if (curr == robot2)
638
                        break;
639
                if (sensor_matrix_get_in_ring(curr))
640
                        count++;
641
                curr++;
642
        }
643
        return count;
644
}
645

    
646
/**
647
 * Passes the token to the next robot in the token ring.
648
 **/
649
static int wl_token_pass_token()
650
{
651
        char nextRobot = 0xFF;
652
        int i = wl_get_xbee_id() + 1;
653
        char buf[2 * sensor_matrix_get_size()];
654
        if (accepted == -1)
655
        {
656
                while (1)
657
                {
658
      //TODO: the compiler may or may not optimize this such that my comment is useless:
659
      // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
660
      // the overhead of a function call each iteration, call it only once before the loop and store
661
      // the value in a variable and check against that variable in the loop condition
662
                        if (i == sensor_matrix_get_size()) {
663
                                i = 0;
664
                        }
665

    
666
                        if (sensor_matrix_get_in_ring(i))
667
                        {
668
                                nextRobot = (char)i;
669
                                break;
670
                        }
671

    
672
                        i++;
673
                }
674
        }
675
        else
676
        {
677
                WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");
678
                //add a new robot to the token ring
679
                sensor_matrix_set_in_ring(accepted, 1);
680
                nextRobot = accepted;
681
                accepted = -1;
682
        }
683

    
684
        int j = 0;
685
  //TODO: the compiler may or may not optimize this such that my comment is useless:
686
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
687
  // the overhead of a function call each iteration, call it only once before the loop and store
688
  // the value in a variable and check against that variable in the loop condition
689
        for (i = 0; i < sensor_matrix_get_size(); i++) {
690
                if (sensor_matrix_get_in_ring(i) && i != wl_get_xbee_id())
691
                {
692
                        buf[2*j] = i;
693
                        buf[2*j + 1] = sensor_matrix_get_reading(wl_get_xbee_id(), i);
694
                        j++;
695
                }
696
        }
697

    
698
        int packetSize = 2 * j * sizeof(char);
699
        WL_DEBUG_PRINT("Passing the token to robot ");
700
        WL_DEBUG_PRINT_INT(nextRobot);
701
        WL_DEBUG_PRINT(".\r\n");
702
        if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0)
703
                return -1;
704
        if (wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME))
705
                return -1;
706

    
707
        wl_token_next_robot = nextRobot;
708
        deathDelay = DEATH_DELAY;
709

    
710
        return 0;
711
}
712

    
713
/**
714
 * Called when a packet is received stating that another robot has turned
715
 * its BOM on. Our BOM is then read, and the data is added to the sensor
716
 * matrix.
717
 *
718
 * @param source the robot whose BOM is on
719
 **/
720
static void wl_token_bom_on_receive(int source)
721
{
722
        int max;
723

    
724
        WL_DEBUG_PRINT("Robot ");
725
        WL_DEBUG_PRINT_INT(source);
726
        WL_DEBUG_PRINT(" has flashed its bom.\r\n");
727

    
728
        bom_on_count = 0;
729

    
730
        max = get_max_bom_function();
731
        sensor_matrix_set_reading(wl_get_xbee_id(),
732
                source, max);
733

    
734
        WL_DEBUG_PRINT("Max: ");
735
        WL_DEBUG_PRINT_INT(max);
736
        WL_DEBUG_PRINT("\n\n");
737
}
738

    
739
/**
740
 * This method is called when we receive the token. Upon receiving
741
 * the token, we must send a BOM_ON packet, flash the BOM, and send
742
 * the token to the next robot.
743
 *
744
 * If there is a pending request for the token, this is processed first.
745
 **/
746
static void wl_token_get_token()
747
{
748
        WL_DEBUG_PRINT("We have the token.\r\n");
749
        if (ringState == ACCEPTED)
750
        {
751
                sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
752
                WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
753
                ringState = MEMBER;
754
                joinDelay = -1;
755
        }
756

    
757
        if (ringState == LEAVING || ringState == NONMEMBER)
758
        {
759
                sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
760
                if (ringState == NONMEMBER)
761
                {
762
                        WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
763
                }
764
                return;
765
        }
766

    
767
        WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
768
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON, NULL, 0, 0);
769

    
770
        bom_on_function();
771
        #ifdef ROBOT
772
        delay_ms(BOM_DELAY);
773
        #endif
774
        bom_off_function();
775

    
776
        if (!sensor_matrix_get_in_ring(wl_get_xbee_id()))
777
        {
778
                WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
779
                return;
780
        }
781

    
782
        wl_token_pass_token();
783
}
784

    
785
/**
786
 * Called when a request to join the token ring is received.
787
 * If we are the robot preceding the requester in the ring,
788
 * we respond with a JOIN_ACCEPT packet and pass the token to
789
 * this robot when we receive the token.
790
 *
791
 * @param source the robot who requested to join
792
 **/
793
static void wl_token_join_receive(int source)
794
{
795
        WL_DEBUG_PRINT("Received joining request from robot ");
796
        WL_DEBUG_PRINT_INT(source);
797
        WL_DEBUG_PRINT(".\r\n");
798

    
799
        //we cannot accept the request if we are not a member
800
        if (ringState != MEMBER)
801
                return;
802
        //if they didn't get our response, see if we should respond again
803
        if (accepted == source)
804
                accepted = -1;
805
        //we can only accept one request at a time
806
        if (accepted != -1)
807
                return;
808

    
809
        //check if we are the preceding robot in the token ring
810
        int i = source - 1;
811
        while (1)
812
        {
813
                if (i < 0)
814
                        i = sensor_matrix_get_size() - 1;
815

    
816
                //we must send a join acceptance
817
                if (i == wl_get_xbee_id())
818
                        break;
819

    
820
                //another robot will handle it
821
                if (sensor_matrix_get_in_ring(i))
822
                        return;
823

    
824
                i--;
825
        }
826

    
827
        accepted = source;
828
        wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT,
829
                NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME);
830

    
831
        WL_DEBUG_PRINT("Accepting robot ");
832
        WL_DEBUG_PRINT_INT(source);
833
        WL_DEBUG_PRINT(" into the token ring.\r\n");
834

    
835
        // the token ring has not started yet
836
        if (sensor_matrix_get_joined() == 1)
837
                wl_token_pass_token();
838
}
839

    
840
/**
841
 * Called when we receive a JOIN_ACCEPT packet in attempting to join
842
 * the token ring.
843
 * Our attempt to join the ring is stopped, and we wait for the token.
844
 *
845
 * @param source the robot who accepted us
846
 **/
847
static void wl_token_join_accept_receive(int source)
848
{
849
        WL_DEBUG_PRINT("Accepted into the token ring by robot ");
850
        WL_DEBUG_PRINT_INT(source);
851
        WL_DEBUG_PRINT(".\r\n");
852
        joinDelay = JOIN_DELAY;
853
        ringState = ACCEPTED;
854
        acceptor = source;
855

    
856
        //add ourselves to the token ring
857
        sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
858
}