Statistics
| Revision:

root / branches / colonetmk2 / code / projects / libwireless / lib / wl_token_ring.c @ 1456

History | View | Annotate | Download (25.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
//TODO: why is this in both this file and sensor_matrix.c?  If it is needed in both places,
51
// put it in sensor_matrix.h.  This file already includes sensor_matrix.h
52
#define DEFAULT_SENSOR_MATRIX_SIZE 20
53

    
54
/*Ring States*/
55

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

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

    
66
/*Function Prototypes*/
67

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

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

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

    
86
/*Global Variables*/
87

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

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

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

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

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

    
109
#if (!(defined ROBOT)) || (defined FIREFLY)
110
static void do_nothing(void) {}
111
static SensorReading get_nothing(void) { SensorReading ret = { READING_UNKNOWN, READING_UNKNOWN }; return ret;}
112
#else
113
static SensorReading default_get_bom(void) { int dist, dir; bom_refresh(BOM_ALL); dir = bom_get_max10(&dist); SensorReading ret = { dir, dist}; return ret; }
114
#endif
115

    
116
#ifdef ROBOT
117
#ifndef FIREFLY
118
static void (*bom_on_function) (void) = bom_on;
119
static void (*bom_off_function) (void) = bom_off;
120
static SensorReading (*get_max_bom_function) (void) = default_get_bom;
121
#else
122
static void (*bom_on_function) (void) = do_nothing;
123
static void (*bom_off_function) (void) = do_nothing;
124
static SensorReading (*get_max_bom_function) (void) = get_nothing;
125
#endif
126
#else
127
static void (*bom_on_function) (void) = do_nothing;
128
static void (*bom_off_function) (void) = do_nothing;
129
static SensorReading (*get_max_bom_function) (void) = get_nothing;
130
#endif
131

    
132
static PacketGroupHandler wl_token_ring_handler =
133
        {WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
134
                wl_token_ring_response_handler, wl_token_ring_receive_handler,
135
                wl_token_ring_cleanup};
136

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

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

    
152
        return 0;
153
}
154

    
155
/**
156
 * Causes the robot to leave the token ring. The robot stops
157
 * alerting others of its location, but continues storing the
158
 * locations of other robots.
159
 **/
160
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
161
// it reduces code size or not should be done to be sure.
162
void wl_token_ring_leave()
163
{
164
        ringState = LEAVING;
165
}
166

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

    
183
        sensor_matrix_create();
184
        //add ourselves to the sensor matrix
185
        sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
186

    
187
        wl_register_packet_group(&wl_token_ring_handler);
188

    
189
        return 0;
190
}
191

    
192
/**
193
 * Removes the packet group from the wireless library.
194
 **/
195
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
196
// it reduces code size or not should be done to be sure.
197
void wl_token_ring_unregister()
198
{
199
        wl_unregister_packet_group(&wl_token_ring_handler);
200
}
201

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

    
222
/**
223
 * Called to cleanup the token ring packet group.
224
 **/
225
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
226
// it reduces code size or not should be done to be sure.
227
static void wl_token_ring_cleanup()
228
{
229
}
230

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

    
251
                // we may have been dropped from the ring when this is received
252
                if (ringState == MEMBER) {
253
                        wl_token_pass_token();
254
                }
255
        }
256

    
257
        //we must start our own token ring, no one is responding to us
258
        if (joinDelay == 0)
259
        {
260
                if (sensor_matrix_get_joined() == 0)
261
                {
262
                        WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");
263
                        sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
264
                        ringState = MEMBER;
265
                        //this will make us pass the token to ourself
266
                        //repeatedly, and other robots when they join
267
                        deathDelay = DEATH_DELAY;
268
                        wl_token_next_robot = wl_get_xbee_id();
269
                }
270
                else
271
                {
272
                        WL_DEBUG_PRINT("Attempting to join the token ring again.\r\n");
273
                        //attempt to rejoin with a random delay
274
                        wl_token_ring_join();
275
                        joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;
276
                }
277
        }
278

    
279
        if (deathDelay >= 0) {
280
                deathDelay--;
281
        }
282

    
283
        if (joinDelay >= 0) {
284
                joinDelay--;
285
        }
286

    
287
        if (bom_on_count >= 0) {
288
                bom_on_count++;
289
        }
290
}
291

    
292
/**
293
 * Called when the XBee tells us if a packet we sent has been received.
294
 *
295
 * @param frame the frame number assigned when the packet was sent
296
 * @param received 1 if the packet was received, 0 otherwise
297
 **/
298
static void wl_token_ring_response_handler(int frame, int received)
299
{
300
        if (!received)
301
        {
302
                WL_DEBUG_PRINT("FAILED.\r\n");
303
        }
304
}
305

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

    
339
/**
340
 * Returns the BOM reading robot source has for robot dest.
341
 *
342
 * @param source the robot that made the BOM reading
343
 * @param dest the robot whose relative location is returned
344
 *
345
 * @return a BOM reading from robot source to robot dest,
346
 * in the range 0-15, or -1 if it is unknown
347
 **/
348
SensorReading * wl_token_get_sensor_reading(int source, int dest)
349
{
350
        if (wl_token_is_robot_in_ring(dest) &&
351
                        (source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source))) {
352
                return sensor_matrix_get_reading(source, dest);
353
        }
354

    
355
        return NULL;
356
}
357

    
358
/**
359
 * Returns the BOM reading we have for robot dest.
360
 *
361
 * @param dest the robot whose relative location is returned
362
 *
363
 * @return a BOM reading from us to robot dest, in the range
364
 * 0-15, or -1 if it is unkown
365
 **/
366
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
367
// it reduces code size or not should be done to be sure.
368
SensorReading * wl_token_get_my_sensor_reading(int dest)
369
{
370
        return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
371
}
372

    
373

    
374
/**
375
 * Returns the number of robots in the token ring.
376
 *
377
 * @return the number of robots in the token ring
378
 **/
379
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
380
// it reduces code size or not should be done to be sure.
381
int wl_token_get_robots_in_ring(void)
382
{
383
        return sensor_matrix_get_joined();
384
}
385

    
386
/**
387
 * Returns true if the specified robot is in the token ring, false
388
 * otherwise.
389
 *
390
 * @param robot the robot to check for whether it is in the token ring
391
 * @return nonzero if the robot is in the token ring, zero otherwise
392
 **/
393
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
394
// it reduces code size or not should be done to be sure.
395
int wl_token_is_robot_in_ring(int robot)
396
{
397
        return sensor_matrix_get_in_ring(robot);
398
}
399

    
400
/**
401
 * Begins iterating through the robots in the token ring.
402
 *
403
 * @see wl_token_iterator_has_next, wl_token_iterator_next
404
 **/
405
void wl_token_iterator_begin(void)
406
{
407
        int i = 0;
408
  //TODO: the compiler may or may not optimize this such that my comment is useless:
409
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
410
  // the overhead of a function call each iteration, call it only once before the loop and store
411
  // the value in a variable and check against that variable in the loop condition
412
        while (!sensor_matrix_get_in_ring(i) && i < sensor_matrix_get_size()) {
413
                i++;
414
        }
415

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

    
421
        iteratorCount = i;
422
}
423

    
424
/**
425
 * Returns true if there are more robots in the token ring
426
 * to iterate through, and false otherwise.
427
 *
428
 * @return nonzero if there are more robots to iterate through,
429
 * zero otherwise
430
 *
431
 * @see wl_token_iterator_begin, wl_token_iterator_next
432
 **/
433
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
434
// it reduces code size or not should be done to be sure.
435
int wl_token_iterator_has_next(void)
436
{
437
        return iteratorCount != -1;
438
}
439

    
440
/**
441
 * Returns the next robot ID in the token ring.
442
 *
443
 * @return the next robot ID in the token ring, or -1 if none exists
444
 *
445
 * @see wl_token_iterator_begin, wl_token_iterator_has_next
446
 **/
447
int wl_token_iterator_next(void)
448
{
449
        int result = iteratorCount;
450
        if (result < 0) {
451
                return result;
452
        }
453

    
454
  //TODO: the compiler may or may not optimize this such that my comment is useless:
455
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
456
  // the overhead of a function call each iteration, call it only once before the loop and store
457
  // the value in a variable and check against that variable in the loop condition
458
        iteratorCount++;
459
        while (!sensor_matrix_get_in_ring(iteratorCount)
460
                && iteratorCount < sensor_matrix_get_size()) {
461
                iteratorCount++;
462
        }
463

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

    
469
        return result;
470
}
471

    
472
/**
473
 * Returns the number of robots currently in the token ring.
474
 *
475
 * @return the number of robots in the token ring
476
 **/
477
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
478
// it reduces code size or not should be done to be sure.
479
int wl_token_get_num_robots(void)
480
{
481
        return sensor_matrix_get_joined();
482
}
483

    
484
/**
485
 * Returns the number of robots in the sensor matrix.
486
 *
487
 * @return the number of robots in the sensor matrix
488
 **/
489
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
490
// it reduces code size or not should be done to be sure.
491
int wl_token_get_matrix_size(void)
492
{
493
        return sensor_matrix_get_size();
494
}
495

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

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

    
536
        bom_on_count = -1;
537
        deathDelay = -1;
538
        sensor_matrix_set_in_ring(source, 1);
539

    
540
        //with this packet, we are passed the id of the next robot in the ring
541
        //and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)
542
        j = 0;
543
  //TODO: the compiler may or may not optimize this such that my comment is useless:
544
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
545
  // the overhead of a function call each iteration, call it only once before the loop and store
546
  // the value in a variable and check against that variable in the loop condition
547
        for (i = 0; i < sensor_matrix_get_size(); i++)
548
        {
549
                if (i == source) {
550
                        continue;
551
                }
552

    
553
                //set the sensor information we receive
554
                if (j < sensorDataLength / 2 && sensorData[3 * j] == i)
555
                {
556
                        //the robot we were going to accept has already been accepted
557
                        if (accepted == i)
558
                        {
559
                                accepted = -1;
560
                                WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
561
                        }
562
                        r.dir = sensorData[3 * j + 1];
563
                        r.dist = sensorData[3 * j + 2];
564
                        sensor_matrix_set_reading(source, i, r);
565
                        if (!sensor_matrix_get_in_ring(i))
566
                        {
567
                                WL_DEBUG_PRINT("Robot ");
568
                                WL_DEBUG_PRINT_INT(i);
569
                                WL_DEBUG_PRINT(" has been added to the sensor matrix of robot ");
570
                                WL_DEBUG_PRINT_INT(wl_get_xbee_id());
571
                                WL_DEBUG_PRINT(" due to a packet from robot ");
572
                                WL_DEBUG_PRINT_INT(source);
573
                                WL_DEBUG_PRINT(".\r\n");
574
                        }
575
                        sensor_matrix_set_in_ring(i, 1);
576
                        j++;
577
                }
578
                else
579
                {
580
                        if (sensor_matrix_get_in_ring(i))
581
                        {
582
                                WL_DEBUG_PRINT("Robot ");
583
                                WL_DEBUG_PRINT_INT(i);
584
                                WL_DEBUG_PRINT(" has been removed from the sensor matrix of robot ");
585
                                WL_DEBUG_PRINT_INT(wl_get_xbee_id());
586
                                WL_DEBUG_PRINT(" due to a packet from robot ");
587
                                WL_DEBUG_PRINT_INT(source);
588
                                WL_DEBUG_PRINT(".\r\n");
589
                                sensor_matrix_set_in_ring(i, 0);
590
                        }
591

    
592
                        if (i == wl_get_xbee_id() && ringState == MEMBER)
593
                        {
594
                                ringState = NONMEMBER;
595
                                wl_token_ring_join();
596

    
597
                                WL_DEBUG_PRINT("We have been removed from the ring ");
598
                                WL_DEBUG_PRINT("and are rejoining.\r\n");
599
                        }
600

    
601
                        //the person who accepted us is dead... let's ask again
602
                        if (i == acceptor)
603
                        {
604
                                sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
605
                                ringState = NONMEMBER;
606
                                acceptor = -1;
607
                                wl_token_ring_join();
608
                        }
609
                }
610
        }
611
        
612
        // get the next robot in the token ring
613
        i = source + 1;
614
        while (1)
615
        {
616
                if (i == sensor_matrix_get_size()) {
617
                        i = 0;
618
                }
619

    
620
                if (sensor_matrix_get_in_ring(i) || i == source)
621
                {
622
                        nextRobot = (char)i;
623
                        break;
624
                }
625

    
626
                i++;
627
        }
628

    
629
        if (nextRobot != wl_get_xbee_id())
630
                wl_token_next_robot = nextRobot;
631

    
632
        deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;
633

    
634
        if (sensor_matrix_get_joined() == 0 && ringState == JOINING)
635
                wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME);
636
}
637

    
638
/**
639
 * Gets the distance in the token ring between two robots.
640
 *
641
 * @param robot1 the first robot
642
 * @param robot2 the second robot
643
 *
644
 * @return the number of passes before the token is expected
645
 * to reach robot2 from robot1
646
 **/
647
static int get_token_distance(int robot1, int robot2)
648
{
649
        int curr = robot1 + 1;
650
        int count = 1;
651
        while (1)
652
        {
653
    //TODO: the compiler may or may not optimize this such that my comment is useless:
654
    // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
655
    // the overhead of a function call each iteration, call it only once before the loop and store
656
    // the value in a variable and check against that variable in the loop condition
657
                if (curr == sensor_matrix_get_size())
658
                        curr = 0;
659
                if (curr == robot2)
660
                        break;
661
                if (sensor_matrix_get_in_ring(curr))
662
                        count++;
663
                curr++;
664
        }
665
        return count;
666
}
667

    
668
/**
669
 * Passes the token to the next robot in the token ring.
670
 **/
671
static int wl_token_pass_token()
672
{
673
        char nextRobot = 0xFF;
674
        int i = wl_get_xbee_id() + 1;
675
        char buf[3 * sensor_matrix_get_size()];
676
        if (accepted == -1)
677
        {
678
                while (1)
679
                {
680
      //TODO: the compiler may or may not optimize this such that my comment is useless:
681
      // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
682
      // the overhead of a function call each iteration, call it only once before the loop and store
683
      // the value in a variable and check against that variable in the loop condition
684
                        if (i == sensor_matrix_get_size()) {
685
                                i = 0;
686
                        }
687

    
688
                        if (sensor_matrix_get_in_ring(i))
689
                        {
690
                                nextRobot = (char)i;
691
                                break;
692
                        }
693

    
694
                        i++;
695
                }
696
        }
697
        else
698
        {
699
                WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");
700
                //add a new robot to the token ring
701
                sensor_matrix_set_in_ring(accepted, 1);
702
                nextRobot = accepted;
703
                accepted = -1;
704
        }
705

    
706
        int j = 0;
707
  //TODO: the compiler may or may not optimize this such that my comment is useless:
708
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
709
  // the overhead of a function call each iteration, call it only once before the loop and store
710
  // the value in a variable and check against that variable in the loop condition
711
        for (i = 0; i < sensor_matrix_get_size(); i++) {
712
                if (sensor_matrix_get_in_ring(i) && i != wl_get_xbee_id())
713
                {
714
                        SensorReading *r = sensor_matrix_get_reading(wl_get_xbee_id(), i);
715
                        buf[3*j] = i;
716
                        buf[3*j + 1] = r->dir;
717
                        buf[3*j + 2] = r->dist;
718
                        j++;
719
                }
720
        }
721

    
722
        int packetSize = 3 * j * sizeof(char);
723
        WL_DEBUG_PRINT("Passing the token to robot ");
724
        WL_DEBUG_PRINT_INT(nextRobot);
725
        WL_DEBUG_PRINT(".\r\n");
726
        if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0)
727
                return -1;
728
        if (wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME))
729
                return -1;
730

    
731
        wl_token_next_robot = nextRobot;
732
        deathDelay = DEATH_DELAY;
733

    
734
        return 0;
735
}
736

    
737
/**
738
 * Called when a packet is received stating that another robot has turned
739
 * its BOM on. Our BOM is then read, and the data is added to the sensor
740
 * matrix.
741
 *
742
 * @param source the robot whose BOM is on
743
 **/
744
static void wl_token_bom_on_receive(int source)
745
{
746
        SensorReading r;
747

    
748
        WL_DEBUG_PRINT("Robot ");
749
        WL_DEBUG_PRINT_INT(source);
750
        WL_DEBUG_PRINT(" has flashed its bom.\r\n");
751

    
752
        bom_on_count = 0;
753

    
754
        r = get_max_bom_function();
755
        sensor_matrix_set_reading(wl_get_xbee_id(),
756
                source, r);
757

    
758
        WL_DEBUG_PRINT("Max: ");
759
        WL_DEBUG_PRINT_INT(r.dir);
760
        WL_DEBUG_PRINT("\tDist: ");
761
        WL_DEBUG_PRINT_INT(r.dist);
762
        WL_DEBUG_PRINT("\n\n");
763
}
764

    
765
/**
766
 * This method is called when we receive the token. Upon receiving
767
 * the token, we must send a BOM_ON packet, flash the BOM, and send
768
 * the token to the next robot.
769
 *
770
 * If there is a pending request for the token, this is processed first.
771
 **/
772
static void wl_token_get_token()
773
{
774
        WL_DEBUG_PRINT("We have the token.\r\n");
775
        if (ringState == ACCEPTED)
776
        {
777
                sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
778
                WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
779
                ringState = MEMBER;
780
                joinDelay = -1;
781
        }
782

    
783
        if (ringState == LEAVING || ringState == NONMEMBER)
784
        {
785
                sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
786
                if (ringState == NONMEMBER)
787
                {
788
                        WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
789
                }
790
                return;
791
        }
792

    
793
        WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
794
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON, NULL, 0, 0);
795

    
796
        bom_on_function();
797
        #ifdef ROBOT
798
        delay_ms(BOM_DELAY);
799
        #endif
800
        bom_off_function();
801

    
802
        if (!sensor_matrix_get_in_ring(wl_get_xbee_id()))
803
        {
804
                WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
805
                return;
806
        }
807

    
808
        wl_token_pass_token();
809
}
810

    
811
/**
812
 * Called when a request to join the token ring is received.
813
 * If we are the robot preceding the requester in the ring,
814
 * we respond with a JOIN_ACCEPT packet and pass the token to
815
 * this robot when we receive the token.
816
 *
817
 * @param source the robot who requested to join
818
 **/
819
static void wl_token_join_receive(int source)
820
{
821
        WL_DEBUG_PRINT("Received joining request from robot ");
822
        WL_DEBUG_PRINT_INT(source);
823
        WL_DEBUG_PRINT(".\r\n");
824

    
825
        //we cannot accept the request if we are not a member
826
        if (ringState != MEMBER)
827
                return;
828
        //if they didn't get our response, see if we should respond again
829
        if (accepted == source)
830
                accepted = -1;
831
        //we can only accept one request at a time
832
        if (accepted != -1)
833
                return;
834

    
835
        //check if we are the preceding robot in the token ring
836
        int i = source - 1;
837
        while (1)
838
        {
839
                if (i < 0)
840
                        i = sensor_matrix_get_size() - 1;
841

    
842
                //we must send a join acceptance
843
                if (i == wl_get_xbee_id())
844
                        break;
845

    
846
                //another robot will handle it
847
                if (sensor_matrix_get_in_ring(i))
848
                        return;
849

    
850
                i--;
851
        }
852

    
853
        accepted = source;
854
        wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT,
855
                NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME);
856

    
857
        WL_DEBUG_PRINT("Accepting robot ");
858
        WL_DEBUG_PRINT_INT(source);
859
        WL_DEBUG_PRINT(" into the token ring.\r\n");
860

    
861
        // the token ring has not started yet
862
        if (sensor_matrix_get_joined() == 1)
863
                wl_token_pass_token();
864
}
865

    
866
/**
867
 * Called when we receive a JOIN_ACCEPT packet in attempting to join
868
 * the token ring.
869
 * Our attempt to join the ring is stopped, and we wait for the token.
870
 *
871
 * @param source the robot who accepted us
872
 **/
873
static void wl_token_join_accept_receive(int source)
874
{
875
        WL_DEBUG_PRINT("Accepted into the token ring by robot ");
876
        WL_DEBUG_PRINT_INT(source);
877
        WL_DEBUG_PRINT(".\r\n");
878
        joinDelay = JOIN_DELAY;
879
        ringState = ACCEPTED;
880
        acceptor = source;
881

    
882
        //add ourselves to the token ring
883
        sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
884
}