Statistics
| Revision:

root / trunk / code / projects / libwireless / lib / wl_token_ring.c @ 1443

History | View | Annotate | Download (24.8 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
#include <stdio.h>
39

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

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

    
51
//TODO: why is this in both this file and sensor_matrix.c?  If it is needed in both places,
52
// put it in sensor_matrix.h.  This file already includes sensor_matrix.h
53
#define DEFAULT_SENSOR_MATRIX_SIZE 20
54

    
55
/*Ring States*/
56

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

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

    
67
/*Function Prototypes*/
68

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

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

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

    
87
/*Global Variables*/
88

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

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

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

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

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

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

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

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

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

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

    
151
        return 0;
152
}
153

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

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

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

    
186
        wl_register_packet_group(&wl_token_ring_handler);
187

    
188
        return 0;
189
}
190

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

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

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

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

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

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

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

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

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

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

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

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

    
354
        return -1;
355
}
356

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

    
372

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

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

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

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

    
420
        iteratorCount = i;
421
}
422

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

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

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

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

    
468
        return result;
469
}
470

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

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

    
495
/**
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.
523
 * @param source is the robot it came from
524
 * @param nextRobot is the robot the token was passed to
525
 * @param sensorData a char with an id followed by a char with the sensor
526
 *                reading for that robot, repeated for sensorDataLength bytes
527
 * @param sensorDataLength the length in bytes of sensorData
528
 */
529
static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength)
530
{
531
        int i, j;
532
        char nextRobot;
533

    
534
        bom_on_count = -1;
535
        deathDelay = -1;
536
        sensor_matrix_set_in_ring(source, 1);
537

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

    
551
                //set the sensor information we receive
552
                if (j < sensorDataLength / 2 && sensorData[2 * j] == i)
553
                {
554
                        //the robot we were going to accept has already been accepted
555
                        if (accepted == i)
556
                        {
557
                                accepted = -1;
558
                                WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
559
                        }
560
                        sensor_matrix_set_reading(source, i,
561
                                                sensorData[2 * j + 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);
573
                        j++;
574
                }
575
                else
576
                {
577
                        if (sensor_matrix_get_in_ring(i))
578
                        {
579
                                WL_DEBUG_PRINT("Robot ");
580
                                WL_DEBUG_PRINT_INT(i);
581
                                WL_DEBUG_PRINT(" has been removed from the sensor matrix of robot ");
582
                                WL_DEBUG_PRINT_INT(wl_get_xbee_id());
583
                                WL_DEBUG_PRINT(" due to a packet from robot ");
584
                                WL_DEBUG_PRINT_INT(source);
585
                                WL_DEBUG_PRINT(".\r\n");
586
                                sensor_matrix_set_in_ring(i, 0);
587
                        }
588

    
589
                        if (i == wl_get_xbee_id() && ringState == MEMBER)
590
                        {
591
                                ringState = NONMEMBER;
592
                                wl_token_ring_join();
593

    
594
                                WL_DEBUG_PRINT("We have been removed from the ring ");
595
                                WL_DEBUG_PRINT("and are rejoining.\r\n");
596
                        }
597

    
598
                        //the person who accepted us is dead... let's ask again
599
                        if (i == acceptor)
600
                        {
601
                                sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
602
                                ringState = NONMEMBER;
603
                                acceptor = -1;
604
                                wl_token_ring_join();
605
                        }
606
                }
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
                }
616

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

    
623
                i++;
624
        }
625

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

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

    
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);
633
}
634

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

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

    
685
                        if (sensor_matrix_get_in_ring(i))
686
                        {
687
                                nextRobot = (char)i;
688
                                break;
689
                        }
690

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

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

    
717
        int packetSize = 2 * j * sizeof(char);
718
        WL_DEBUG_PRINT("Passing the token to robot ");
719
        WL_DEBUG_PRINT_INT(nextRobot);
720
        WL_DEBUG_PRINT(".\r\n");
721
        if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0)
722
                return -1;
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;
725

    
726
        wl_token_next_robot = nextRobot;
727
        deathDelay = DEATH_DELAY;
728

    
729
        return 0;
730
}
731

    
732
/**
733
 * Called when a packet is received stating that another robot has turned
734
 * its BOM on. Our BOM is then read, and the data is added to the sensor
735
 * matrix.
736
 *
737
 * @param source the robot whose BOM is on
738
 **/
739
static void wl_token_bom_on_receive(int source)
740
{
741
        int max, dist;
742

    
743
        WL_DEBUG_PRINT("Robot ");
744
        WL_DEBUG_PRINT_INT(source);
745
        WL_DEBUG_PRINT(" has flashed its bom.\r\n");
746

    
747
        bom_on_count = 0;
748

    
749
        max = get_max_bom_function();
750
        sensor_matrix_set_reading(wl_get_xbee_id(),
751
                source, max);
752

    
753
        WL_DEBUG_PRINT("Max: ");
754
        WL_DEBUG_PRINT_INT(max);
755
        WL_DEBUG_PRINT("\tVar: ");
756
        WL_DEBUG_PRINT_INT(dist);
757
        WL_DEBUG_PRINT("\n\n");
758
}
759

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

    
778
        if (ringState == LEAVING || ringState == NONMEMBER)
779
        {
780
                sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
781
                if (ringState == NONMEMBER)
782
                {
783
                        WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
784
                }
785
                return;
786
        }
787

    
788
        WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
789
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON, NULL, 0, 0);
790

    
791
        bom_on_function();
792
        #ifdef ROBOT
793
        delay_ms(BOM_DELAY);
794
        #endif
795
        bom_off_function();
796

    
797
        if (!sensor_matrix_get_in_ring(wl_get_xbee_id()))
798
        {
799
                WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
800
                return;
801
        }
802

    
803
        wl_token_pass_token();
804
}
805

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

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

    
830
        //check if we are the preceding robot in the token ring
831
        int i = source - 1;
832
        while (1)
833
        {
834
                if (i < 0)
835
                        i = sensor_matrix_get_size() - 1;
836

    
837
                //we must send a join acceptance
838
                if (i == wl_get_xbee_id())
839
                        break;
840

    
841
                //another robot will handle it
842
                if (sensor_matrix_get_in_ring(i))
843
                        return;
844

    
845
                i--;
846
        }
847

    
848
        accepted = source;
849
        wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT,
850
                NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME);
851

    
852
        WL_DEBUG_PRINT("Accepting robot ");
853
        WL_DEBUG_PRINT_INT(source);
854
        WL_DEBUG_PRINT(" into the token ring.\r\n");
855

    
856
        // the token ring has not started yet
857
        if (sensor_matrix_get_joined() == 1)
858
                wl_token_pass_token();
859
}
860

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

    
877
        //add ourselves to the token ring
878
        sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
879
}