Project

General

Profile

Statistics
| Revision:

root / trunk / code / lib / src / libwireless / wl_token_ring.c @ 86

History | View | Annotate | Download (20.9 KB)

1
#include <wl_token_ring.h>
2

    
3
#include <stdlib.h>
4
#include <stdio.h>
5

    
6
#include <wl_defs.h>
7
#include <wireless.h>
8
#include <sensor_matrix.h>
9
#include <queue.h>
10

    
11
#ifdef ROBOT
12
#ifndef FIREFLY
13
#include <bom.h>
14
#endif
15
#include <time.h>
16
#endif
17

    
18
#define DEFAULT_SENSOR_MATRIX_SIZE 20
19

    
20
/*Ring States*/
21

    
22
#define NONMEMBER 0
23
#define MEMBER 1
24
#define JOINING 2
25
#define ACCEPTED 3
26
#define LEAVING 4
27

    
28
/*Frame Types*/
29
#define TOKEN_JOIN_ACCEPT_FRAME 1
30

    
31
/*Function Prototypes*/
32

    
33
/*Wireless Library Prototypes*/
34
void wl_token_ring_timeout_handler(void);
35
void wl_token_ring_response_handler(int frame, int received);
36
void wl_token_ring_receive_handler(char type, int source, unsigned char* packet,
37
                                                        int length);
38
void wl_token_ring_cleanup(void);
39

    
40
/*Helper Functions*/
41
void wl_token_pass_token(void);
42
int get_token_distance(int robot1, int robot2);
43
void wl_token_get_token(void);
44

    
45
/*Packet Handling Routines*/
46
void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength);
47
void wl_token_interrupt_request_receive(int source, int robot);
48
void wl_token_interrupt_pass_receive(int source, int robot);
49
void wl_token_bom_on_receive(int source);
50
void wl_token_join_receive(int source);
51
void wl_token_join_accept_receive(int source);
52

    
53
/*Global Variables*/
54

    
55
//the sensor matrix
56
SensorMatrix* sensorMatrix;
57

    
58
//the robot we are waiting to say it has received the token. -1 if unspecified
59
int wl_token_next_robot = -1;
60

    
61
//true if the robot should be in the token ring, 0 otherwise
62
int ringState = NONMEMBER;
63
//the id of the robot who accepted us into the token ring, only used in ACCEPTED state
64
int acceptor = -1;
65
//id of the robot we are accepting
66
int accepted = -1;
67

    
68
//the counter for when we assume a robot is dead
69
int deathDelay = -1;
70
//the counter for joining, before we form our own token ring
71
int joinDelay = -1;
72
//queue containing ids of interruption requests
73
Queue* interrupting = NULL;
74

    
75
//current robot to check in the iterator
76
int iteratorCount = 0;
77

    
78
void do_nothing(void) {}
79
int get_nothing(void) {return -1;}
80

    
81
#ifdef ROBOT
82
#ifndef FIREFLY
83
void (*bom_on_function) (void) = bom_on;
84
void (*bom_off_function) (void) = bom_off;
85
int (*get_max_bom_function) (void) = get_max_bom;
86
#else
87
void (*bom_on_function) (void) = do_nothing;
88
void (*bom_off_function) (void) = do_nothing;
89
int (*get_max_bom_function) (void) = get_nothing;
90
#endif
91
#else
92
void (*bom_on_function) (void) = do_nothing;
93
void (*bom_off_function) (void) = do_nothing;
94
int (*get_max_bom_function) (void) = get_nothing;
95
#endif
96

    
97
PacketGroupHandler wl_token_ring_handler =
98
                {WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
99
                wl_token_ring_response_handler, wl_token_ring_receive_handler,
100
                wl_token_ring_cleanup};
101

    
102
/**
103
 * Initialize the token ring packet group and register it with the
104
 * wireless library. The robot will not join a token ring.
105
 **/
106
void wl_token_ring_register()
107
{
108
        if (wl_get_xbee_id() > 0xFF)
109
        {
110
                //Note: if this becomes an issue (unlikely), we could limit sensor information
111
                //to half a byte and use 12 bits for the id
112
                WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");
113
                WL_DEBUG_PRINT_INT(wl_get_xbee_id());
114
                WL_DEBUG_PRINT(".\r\n");
115
                return;
116
        }
117
        
118
        sensorMatrix = sensor_matrix_create();
119
        interrupting = queue_create();
120
        //add ourselves to the sensor matrix
121
        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
122

    
123
        wl_register_packet_group(&wl_token_ring_handler);
124
}
125

    
126
/**
127
 * Removes the packet group from the wireless library.
128
 **/
129
void wl_token_ring_unregister()
130
{
131
        wl_unregister_packet_group(&wl_token_ring_handler);
132
}
133

    
134
/**
135
 * Sets the functions that are called when the BOM ought to be
136
 * turned on or off. This could be used for things such as 
137
 * charging stations, which have multiple BOMs.
138
 *
139
 * @param on_function the function to be called when the BOM
140
 * should be turned on
141
 * @param off_function the function to be called when the BOM
142
 * should be turned off
143
 * @param max_bom_function the function to be called when a
144
 * measurement of the maximum BOM reading is needed.
145
 **/
146
void wl_token_ring_set_bom_functions(void (*on_function) (void),
147
        void (*off_function) (void), int (*max_bom_function) (void))
148
{
149
        bom_on_function = on_function;
150
        bom_off_function = off_function;
151
        get_max_bom_function = max_bom_function;
152
}
153

    
154
/**
155
 * Called to cleanup the token ring packet group.
156
 **/
157
void wl_token_ring_cleanup()
158
{
159
        sensor_matrix_destroy(sensorMatrix);
160
        queue_destroy(interrupting);
161
}
162

    
163
/**
164
 * Called approximately every quarter second by the wireless library.
165
 **/
166
void wl_token_ring_timeout_handler()
167
{
168
        //someone is not responding, assume they are dead
169
        if (deathDelay == 0)
170
        {
171
                //pass the token to the next robot if we think someone has died
172
                //also, declare that person dead, as long as it isn't us
173
                if (wl_token_next_robot != wl_get_xbee_id())
174
                {
175
                        sensor_matrix_set_in_ring(sensorMatrix, wl_token_next_robot, 0);
176
                        WL_DEBUG_PRINT("Robot ");
177
                        WL_DEBUG_PRINT_INT(wl_token_next_robot);
178
                        WL_DEBUG_PRINT(" has died.\r\n");
179
                }
180
                
181
                // we may have been dropped from the ring when this is received
182
                if (ringState == MEMBER)
183
                        wl_token_pass_token();
184
        }
185

    
186
        //we must start our own token ring, no one is responding to us
187
        if (joinDelay == 0)
188
        {
189
                if (sensor_matrix_get_joined(sensorMatrix) == 0)
190
                {
191
                        WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");
192
                        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
193
                        ringState = MEMBER;
194
                        //this will make us pass the token to ourself
195
                        //repeatedly, and other robots when they join
196
                        deathDelay = DEATH_DELAY;
197
                        wl_token_next_robot = wl_get_xbee_id();
198
                }
199
                else
200
                {
201
                        WL_DEBUG_PRINT("Attempting to join the token ring again.\r\n");
202
                        //attempt to rejoin with a random delay
203
                        wl_token_ring_join();
204
                        joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;
205
                }
206
        }
207

    
208
        if (deathDelay >= 0)
209
                deathDelay--;
210
        if (joinDelay >= 0)
211
                joinDelay--;
212
}
213

    
214
/**
215
 * Called when the XBee tells us if a packet we sent has been received.
216
 * 
217
 * @param frame the frame number assigned when the packet was sent
218
 * @param received 1 if the packet was received, 0 otherwise
219
 **/
220
void wl_token_ring_response_handler(int frame, int received)
221
{
222
        if (!received)
223
        {
224
                WL_DEBUG_PRINT("FAILED.\r\n");
225
        }
226
}
227

    
228
/**
229
 * Called when we recieve a token ring packet.
230
 * @param type the type of the packet
231
 * @param source the id of the robot who sent the packet
232
 * @param packet the data in the packet
233
 * @param length the length of the packet in bytes
234
 **/
235
void wl_token_ring_receive_handler(char type, int source, unsigned char* packet,
236
                                                        int length)
237
{
238
        switch (type)
239
        {
240
                case WL_TOKEN_PASS:
241
                        if (length < 1)
242
                        {
243
                                WL_DEBUG_PRINT("Malformed Token Pass packet received.\r\n");
244
                                return;
245
                        }
246
                        wl_token_pass_receive(source, packet[0], packet + 1, length - 1);
247
                        break;
248
                case WL_TOKEN_BOM_ON:
249
                        //add the robot to the sensor matrix if it is not already there
250
                        wl_token_bom_on_receive(source);
251
                        break;
252
                case WL_TOKEN_INTERRUPT_REQUEST:
253
                        wl_token_interrupt_request_receive(source, packet[0]);
254
                        break;
255
                case WL_TOKEN_INTERRUPT_PASS:
256
                        wl_token_interrupt_pass_receive(source, packet[0]);
257
                        break;
258
                case WL_TOKEN_JOIN:
259
                        wl_token_join_receive(source);
260
                        break;
261
                case WL_TOKEN_JOIN_ACCEPT:
262
                        wl_token_join_accept_receive(source);
263
                        break;
264
                default:
265
                        WL_DEBUG_PRINT("Unimplemented token ring packet received.\r\n");
266
                        break;
267
        }
268
}
269

    
270
/**
271
 * Causes the robot to join an existing token ring, or create one
272
 * if no token ring exists. The token ring uses global and robot to robot
273
 * packets, and does not rely on any PAN.
274
 **/
275
void wl_token_ring_join()
276
{
277
        WL_DEBUG_PRINT("Joining the token ring.\r\n");
278
        ringState = JOINING;
279
        joinDelay = JOIN_DELAY;
280
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN,
281
                NULL, 0, 0);
282
}
283

    
284
/**
285
 * Causes the robot to leave the token ring. The robot stops
286
 * alerting others of its location, but continues storing the
287
 * locations of other robots.
288
 **/
289
void wl_token_ring_leave()
290
{
291
        ringState = LEAVING;
292
}
293

    
294
/**
295
 * Requests that the specified robot be given the token and
296
 * allowed to flash its BOM. After its BOM is flashed, the
297
 * token will return to the robot who sent it.
298
 *
299
 * @param robot the ID of the robot which should flash its BOM
300
 **/
301
void wl_token_request(int robot)
302
{
303
        char buf[1];
304
        buf[0] = robot;
305
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_REQUEST,
306
                buf, 1, 0);
307
}
308

    
309
/**
310
 * Returns the BOM reading robot source has for robot dest.
311
 *
312
 * @param source the robot that made the BOM reading
313
 * @param dest the robot whose relative location is returned
314
 *
315
 * @return a BOM reading from robot source to robot dest,
316
 * in the range 0-15, or -1 if it is unknown
317
 **/
318
int wl_token_get_sensor_reading(int source, int dest)
319
{
320
        return sensor_matrix_get_reading(sensorMatrix, source, dest);
321
}
322

    
323
/**
324
 * Returns the BOM reading we have for robot dest.
325
 * 
326
 * @param dest the robot whose relative location is returned
327
 *
328
 * @return a BOM reading from us to robot dest, in the range
329
 * 0-15, or -1 if it is unkown
330
 **/
331
int wl_token_get_my_sensor_reading(int dest)
332
{
333
        return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
334
}
335

    
336
/**
337
 * This method is called when we receive a token pass packet.
338
 * @param source is the robot it came from
339
 * @param nextRobot is the robot the token was passed to
340
 * @param sensorData a char with an id followed by a char with the sensor
341
 *                reading for that robot, repeated for sensorDataLength bytes
342
 * @param sensorDataLength the length in bytes of sensorData
343
 */
344
void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength)
345
{
346
        int i, j;
347
        deathDelay = -1;
348

    
349
        WL_DEBUG_PRINT("Received the token, next robot is ");
350
        WL_DEBUG_PRINT_INT((int)nextRobot);
351
        WL_DEBUG_PRINT(" \r\n");
352
        sensor_matrix_set_in_ring(sensorMatrix, source, 1);
353

    
354
        //with this packet, we are passed the id of the next robot in the ring
355
        //and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)
356
        j = 0;
357
        for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
358
        {
359
                if (i == source)
360
                        continue;
361
                
362
                //set the sensor information we receive
363
                if (j < sensorDataLength / 2 && sensorData[2 * j] == i)
364
                {
365
                        //the robot we were going to accept has already been accepted
366
                        if (accepted == i)
367
                        {
368
                                accepted = -1;
369
                                WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
370
                        }
371
                        sensor_matrix_set_reading(sensorMatrix, source, i,
372
                                                sensorData[2 * j + 1]);
373
                        sensor_matrix_set_in_ring(sensorMatrix, i, 1);
374
                        j++;
375
                }
376
                else
377
                {
378
                        if (sensor_matrix_get_in_ring(sensorMatrix, i))
379
                        {
380
                                WL_DEBUG_PRINT("Robot ");
381
                                WL_DEBUG_PRINT_INT(i);
382
                                WL_DEBUG_PRINT(" has been removed from the sensor matrix of robot ");
383
                                WL_DEBUG_PRINT_INT(wl_get_xbee_id());
384
                                WL_DEBUG_PRINT(" due to a packet from robot ");
385
                                WL_DEBUG_PRINT_INT(source);
386
                                WL_DEBUG_PRINT(".\r\n");
387
                                sensor_matrix_set_in_ring(sensorMatrix, i, 0);
388
                        }
389

    
390
                        if (i == wl_get_xbee_id() && ringState == MEMBER)
391
                        {
392
                                ringState = NONMEMBER;
393
                                wl_token_ring_join();
394
                                
395
                                WL_DEBUG_PRINT("We have been removed from the ring ");
396
                                WL_DEBUG_PRINT("and are rejoining.\r\n");
397
                        }
398
                        
399
                        //the person who accepted us is dead... let's ask again
400
                        if (i == acceptor)
401
                        {
402
                                sensor_matrix_set_in_ring(sensorMatrix,
403
                                                wl_get_xbee_id(), 1);
404
                                ringState = NONMEMBER;
405
                                acceptor = -1;
406
                                wl_token_ring_join();
407
                        }
408
                }
409
        }
410

    
411
        wl_token_next_robot = nextRobot;
412
        
413
        deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;
414
        
415
        //we have the token
416
        if (wl_token_next_robot == wl_get_xbee_id())
417
                wl_token_get_token();
418
}
419

    
420
/**
421
 * Gets the distance in the token ring between two robots.
422
 *
423
 * @param robot1 the first robot
424
 * @param robot2 the second robot
425
 *
426
 * @return the number of passes before the token is expected
427
 * to reach robot2 from robot1
428
 **/
429
int get_token_distance(int robot1, int robot2)
430
{
431
        int curr = robot1 + 1;
432
        int count = 1;
433
        while (1)
434
        {
435
                if (curr == sensor_matrix_get_size(sensorMatrix))
436
                        curr = 0;
437
                if (curr == robot2)
438
                        break;
439
                if (sensor_matrix_get_in_ring(sensorMatrix, curr))
440
                        count++;
441
                curr++;
442
        }
443
        return count;
444
}
445

    
446
/**
447
 * Passes the token to the next robot in the token ring.
448
 **/
449
void wl_token_pass_token()
450
{
451
        char nextRobot;
452
        int i = wl_get_xbee_id() + 1;
453
        if (accepted == -1)
454
        {
455
                while (1)
456
                {
457
                        if (i == sensor_matrix_get_size(sensorMatrix))
458
                                i = 0;
459
                        if (sensor_matrix_get_in_ring(sensorMatrix, i))
460
                        {
461
                                nextRobot = (char)i;
462
                                break;
463
                        }
464
                        i++;
465
                }
466
        }
467
        else
468
        {
469
                WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");
470
                //add a new robot to the token ring
471
                sensor_matrix_set_in_ring(sensorMatrix, accepted, 1);
472
                nextRobot = accepted;
473
                accepted = -1;
474
        }
475

    
476
        //we don't include ourself
477
        int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
478
        char* buf = (char*)malloc(packetSize * sizeof(char));
479
        if (!buf)
480
        {
481
                WL_DEBUG_PRINT_INT(packetSize);
482
                WL_DEBUG_PRINT("Out of memory - pass token.\r\n");
483
                return;
484
        }
485
        buf[0] = nextRobot;
486

    
487
        int j = 0;
488
        for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
489
                if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
490
                {
491
                        buf[2*j + 1] = i;
492
                        buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);
493
                        j++;
494
                }
495
        
496
        WL_DEBUG_PRINT("Passing the token to robot ");
497
        WL_DEBUG_PRINT_INT(buf[0]);
498
        WL_DEBUG_PRINT(".\r\n");
499
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,
500
                buf, packetSize, 3);
501

    
502
        wl_token_next_robot = nextRobot;
503
        deathDelay = DEATH_DELAY;
504
        free(buf);
505
}
506

    
507
/**
508
 * Called when a packet is received stating that another robot has turned
509
 * its BOM on. Our BOM is then read, and the data is added to the sensor
510
 * matrix.
511
 *
512
 * @param source the robot whose BOM is on
513
 **/
514
void wl_token_bom_on_receive(int source)
515
{
516
        WL_DEBUG_PRINT("Robot ");
517
        WL_DEBUG_PRINT_INT(source);
518
        WL_DEBUG_PRINT(" has flashed its bom.\r\n");
519
        sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(), 
520
                source, get_max_bom_function());
521
}
522

    
523
/**
524
 * This method is called when we receive the token. Upon receiving
525
 * the token, we must send a BOM_ON packet, flash the BOM, and send
526
 * the token to the next robot.
527
 * 
528
 * If there is a pending request for the token, this is processed first.
529
 **/
530
void wl_token_get_token()
531
{
532
        WL_DEBUG_PRINT("We have the token.\r\n");
533
        if (ringState == ACCEPTED)
534
        {
535
                sensor_matrix_set_in_ring(sensorMatrix,
536
                        wl_get_xbee_id(), 1);
537
                WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
538
                ringState = MEMBER;
539
        }
540

    
541
        if (ringState == LEAVING || ringState == NONMEMBER)
542
        {
543
                sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
544
                if (ringState == NONMEMBER)
545
                {
546
                        WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
547
                }
548
                return;
549
        }
550
        
551
        //check for interruption requests
552
        if (queue_size(interrupting) > 0)
553
        {
554
                char buf[1];
555
                buf[0] = (char)(int)queue_remove(interrupting);
556
                
557
                //in case this robot has requested multiple times
558
                queue_remove_all(interrupting, (void*)(int)buf[0]);
559

    
560
                wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_PASS,
561
                        buf, 1, 0);
562

    
563
                deathDelay = DEATH_DELAY;
564
                wl_token_next_robot = buf[0];
565
                return;
566
        }
567

    
568
        WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
569
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,
570
                NULL, 0, 0);
571

    
572
        bom_on_function();
573
        #ifdef ROBOT
574
        delay_ms(BOM_DELAY);
575
        #endif
576
        bom_off_function();
577
        
578
        if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
579
        {
580
                WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
581
        }
582
        
583
        wl_token_pass_token();
584
}
585

    
586
/**
587
 * Called when a request to join the token ring is received.
588
 * If we are the robot preceding the requester in the ring,
589
 * we respond with a JOIN_ACCEPT packet and pass the token to
590
 * this robot when we receive the token.
591
 *
592
 * @param source the robot who requested to join
593
 **/
594
void wl_token_join_receive(int source)
595
{
596
        WL_DEBUG_PRINT("Received joining request from robot ");
597
        WL_DEBUG_PRINT_INT(source);
598
        WL_DEBUG_PRINT(".\r\n");
599

    
600
        //we cannot accept the request if we are not a member
601
        if (ringState != MEMBER)
602
                return;
603
        //if they didn't get our response, see if we should respond again
604
        if (accepted == source)
605
                accepted = -1;
606
        //we can only accept one request at a time
607
        if (accepted != -1)
608
                return;
609
        
610
        //check if we are the preceding robot in the token ring
611
        int i = source - 1;
612
        while (1)
613
        {
614
                if (i < 0)
615
                        i = sensor_matrix_get_size(sensorMatrix) - 1;
616
                //we must send a join acceptance
617
                if (i == wl_get_xbee_id())
618
                        break;
619

    
620
                //another robot will handle it
621
                if (sensor_matrix_get_in_ring(sensorMatrix, i))
622
                        return;
623
                i--;
624
        }
625

    
626
        accepted = source;
627
        wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT,
628
                NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME);
629
        
630
        WL_DEBUG_PRINT("Accepting robot ");
631
        WL_DEBUG_PRINT_INT(source);
632
        WL_DEBUG_PRINT(" into the token ring.\r\n");
633

    
634
        joinDelay = -1;
635
        
636
        // the token ring has not started yet
637
        if (sensor_matrix_get_joined(sensorMatrix) == 1)
638
                wl_token_pass_token();
639
}
640

    
641
/**
642
 * Called when we receive a JOIN_ACCEPT packet in attempting to join
643
 * the token ring.
644
 * Our attempt to join the ring is stopped, and we wait for the token.
645
 *
646
 * @param source the robot who accepted us
647
 **/
648
void wl_token_join_accept_receive(int source)
649
{
650
        WL_DEBUG_PRINT("Accepted into the token ring by robot ");
651
        WL_DEBUG_PRINT_INT(source);
652
        WL_DEBUG_PRINT(".\r\n");
653
        joinDelay = -1;
654
        ringState = ACCEPTED;
655
        acceptor = source;
656

    
657
        //add ourselves to the token ring
658
        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
659
}
660

    
661
/**
662
 * Called when we receive a packet passing the token and interrupting
663
 * the token ring.
664
 * If the token has been passed to us, we flash our BOM
665
 * and pass it back.
666
 *
667
 * @param source the robot who sent the interrupt packet
668
 * @param robot the robot the token has been passed to
669
 **/
670
void wl_token_interrupt_pass_receive(int source, int robot)
671
{
672
        if (wl_get_xbee_id() != robot)
673
        {
674
                queue_remove_all(interrupting, (void*)robot);
675
                wl_token_next_robot = robot;
676
                deathDelay = DEATH_DELAY + rand() / (RAND_MAX / (2 * DEATH_DELAY));
677
                return;
678
        }
679
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,
680
                NULL, 0, 0);
681
        
682
        bom_on_function();
683
        #ifdef ROBOT
684
        delay_ms(BOM_DELAY);
685
        #endif
686
        bom_off_function();
687

    
688
        //we don't include ourself, only if we are in the ring
689
        int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
690
        if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
691
                packetSize += 2;
692
        char* buf = (char*)malloc(packetSize * sizeof(char));
693
        if (!buf)
694
        {
695
                WL_DEBUG_PRINT("Out of memory - pass_receive.\r\n");
696
                return;
697
        }
698
        
699
        //return the token to where we got it from
700
        buf[0] = source;
701

    
702
        int i = 0, j = 0;
703
        for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
704
                if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
705
                {
706
                        buf[2*j + 1] = i;
707
                        buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);
708
                        j++;
709
                }
710
        
711
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,
712
                buf, packetSize, 0);
713

    
714
        wl_token_next_robot = source;
715
        deathDelay = DEATH_DELAY;
716
        free(buf);
717
}
718

    
719
/**
720
 * Returns the number of robots in the token ring.
721
 *
722
 * @return the number of robots in the token ring
723
 **/
724
int wl_token_get_robots_in_ring(void)
725
{
726
        return sensor_matrix_get_joined(sensorMatrix);
727
}
728

    
729
/**
730
 * Returns true if the specified robot is in the token ring, false
731
 * otherwise.
732
 *
733
 * @param robot the robot to check for whether it is in the token ring
734
 * @return nonzero if the robot is in the token ring, zero otherwise
735
 **/
736
int wl_token_is_robot_in_ring(int robot)
737
{
738
        return sensor_matrix_get_in_ring(sensorMatrix, robot);
739
}
740

    
741
/**
742
 * Begins iterating through the robots in the token ring.
743
 *
744
 * @see wl_token_iterator_has_next, wl_token_iterator_next
745
 **/
746
void wl_token_iterator_begin(void)
747
{
748
        int i = 0;
749
        iteratorCount = 0;
750
        while (!sensor_matrix_get_in_ring(sensorMatrix, i) &&
751
                        i < sensor_matrix_get_size(sensorMatrix))
752
                i++;
753
        if (i == sensor_matrix_get_size(sensorMatrix))
754
                i = -1;
755
}
756

    
757
/**
758
 * Returns true if there are more robots in the token ring
759
 * to iterate through, and false otherwise.
760
 *
761
 * @return nonzero if there are more robots to iterate through,
762
 * zero otherwise
763
 *
764
 * @see wl_token_iterator_begin, wl_token_iterator_next
765
 **/
766
int wl_token_iterator_has_next(void)
767
{
768
        return iteratorCount == -1;
769
}
770

    
771
/**
772
 * Returns the next robot ID in the token ring.
773
 *
774
 * @return the next robot ID in the token ring, or -1 if none exists
775
 *
776
 * @see wl_token_iterator_begin, wl_token_iterator_has_next
777
 **/
778
int wl_token_iterator_next(void)
779
{
780
        int result = iteratorCount;
781
        if (result < 0)
782
                return result;
783

    
784
        iteratorCount++;
785
        while (!sensor_matrix_get_in_ring(sensorMatrix, iteratorCount) &&
786
                iteratorCount < sensor_matrix_get_size(sensorMatrix))
787
                iteratorCount++;
788
        if (iteratorCount == sensor_matrix_get_size(sensorMatrix))
789
                iteratorCount = -1;
790
        return result;
791
}
792

    
793
/**
794
 * Called when we receive a request to interrupt the token ring.
795
 * We add the robot to our list of interrupt requests,
796
 * and will send the token to this robot when we next receive the
797
 * token, unless someone else does so first.
798
 *
799
 * @param source the robot requesting interruption
800
 * @param robt the robot requested to interrupt the token ring
801
 **/
802
void wl_token_interrupt_request_receive(int source, int robot)
803
{
804
        queue_add(interrupting, (void*)robot);
805
}
806

    
807
int wl_token_get_num_robots(void){
808
  return sensor_matrix_get_joined(sensorMatrix);
809
}
810

    
811
int wl_token_get_matrix_size(void){
812
  return sensor_matrix_get_size(sensorMatrix);
813
}