Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (19 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
#include <bom.h>
13
#include <time.h>
14
#endif
15

    
16
#define DEFAULT_SENSOR_MATRIX_SIZE 20
17

    
18
/*Ring States*/
19

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

    
26
/*Frame Types*/
27
#define TOKEN_JOIN_ACCEPT_FRAME 1
28

    
29
/*Function Prototypes*/
30

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

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

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

    
51
/*Global Variables*/
52

    
53
//the sensor matrix
54
SensorMatrix* sensorMatrix;
55

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

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

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

    
73
#ifdef ROBOT
74
void (*bom_on_function) (void) = bom_on;
75
void (*bom_off_function) (void) = bom_off;
76
int (*get_max_bom_function) (void) = get_max_bom;
77
#else
78
void do_nothing(void) {}
79
int get_nothing(void) {return -1;}
80
void (*bom_on_function) (void) = do_nothing;
81
void (*bom_off_function) (void) = do_nothing;
82
int (*get_max_bom_function) (void) = get_nothing;
83
#endif
84

    
85
PacketGroupHandler wl_token_ring_handler =
86
                {WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
87
                wl_token_ring_response_handler, wl_token_ring_receive_handler,
88
                wl_token_ring_cleanup};
89

    
90
/**
91
 * Initialize the token ring packet group and register it with the
92
 * wireless library. The robot will not join a token ring.
93
 **/
94
void wl_token_ring_register()
95
{
96
        if (wl_get_xbee_id() > 0xFF)
97
        {
98
                //Note: if this becomes an issue (unlikely), we could limit sensor information
99
                //to half a byte and use 12 bits for the id
100
                WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");
101
                WL_DEBUG_PRINT_INT(wl_get_xbee_id());
102
                WL_DEBUG_PRINT(".\r\n");
103
                return;
104
        }
105
        
106
        sensorMatrix = sensor_matrix_create();
107
        interrupting = queue_create();
108
        //add ourselves to the sensor matrix
109
        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
110

    
111
        wl_register_packet_group(&wl_token_ring_handler);
112
}
113

    
114
/**
115
 * Removes the packet group from the wireless library.
116
 **/
117
void wl_token_ring_unregister()
118
{
119
        wl_unregister_packet_group(&wl_token_ring_handler);
120
}
121

    
122
/**
123
 * Sets the functions that are called when the BOM ought to be
124
 * turned on or off. This could be used for things such as 
125
 * charging stations, which have multiple BOMs.
126
 *
127
 * @param on_function the function to be called when the BOM
128
 * should be turned on
129
 * @param off_function the function to be called when the BOM
130
 * should be turned off
131
 * @param max_bom_function the function to be called when a
132
 * measurement of the maximum BOM reading is needed.
133
 **/
134
void wl_token_ring_set_bom_functions(void (*on_function) (void),
135
        void (*off_function) (void), int (*max_bom_function) (void))
136
{
137
        bom_on_function = on_function;
138
        bom_off_function = off_function;
139
        get_max_bom_function = max_bom_function;
140
}
141

    
142
/**
143
 * Called to cleanup the token ring packet group.
144
 **/
145
void wl_token_ring_cleanup()
146
{
147
        sensor_matrix_destroy(sensorMatrix);
148
        queue_destroy(interrupting);
149
}
150

    
151
/**
152
 * Called approximately every quarter second by the wireless library.
153
 **/
154
void wl_token_ring_timeout_handler()
155
{
156
        //someone is not responding, assume they are dead
157
        if (deathDelay == 0)
158
        {
159
                //pass the token to the next robot if we think someone has died
160
                //also, declare that person dead, as long as it isn't us
161
                if (wl_token_next_robot != wl_get_xbee_id())
162
                {
163
                        sensor_matrix_set_in_ring(sensorMatrix, wl_token_next_robot, 0);
164
                        WL_DEBUG_PRINT("Robot ");
165
                        WL_DEBUG_PRINT_INT(wl_token_next_robot);
166
                        WL_DEBUG_PRINT(" has died.\r\n");
167
                }
168
                
169
                // we may have been dropped from the ring when this is received
170
                if (ringState == MEMBER)
171
                        wl_token_pass_token();
172
        }
173

    
174
        //we must start our own token ring, no one is responding to us
175
        if (joinDelay == 0)
176
        {
177
                if (sensor_matrix_get_joined(sensorMatrix) == 0)
178
                {
179
                        WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");
180
                        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
181
                        ringState = MEMBER;
182
                        //this will make us pass the token to ourself
183
                        //repeatedly, and other robots when they join
184
                        deathDelay = DEATH_DELAY;
185
                        wl_token_next_robot = wl_get_xbee_id();
186
                }
187
                else
188
                {
189
                        WL_DEBUG_PRINT("Attempting to join the token ring again.\r\n");
190
                        //attempt to rejoin with a random delay
191
                        wl_token_ring_join();
192
                        joinDelay = rand() / (RAND_MAX / JOIN_DELAY) + 1;
193
                }
194
        }
195

    
196
        if (deathDelay >= 0)
197
                deathDelay--;
198
        if (joinDelay >= 0)
199
                joinDelay--;
200
}
201

    
202
/**
203
 * Called when the XBee tells us if a packet we sent has been received.
204
 * 
205
 * @param frame the frame number assigned when the packet was sent
206
 * @param received 1 if the packet was received, 0 otherwise
207
 **/
208
void wl_token_ring_response_handler(int frame, int received)
209
{
210
        if (!received)
211
        {
212
                WL_DEBUG_PRINT("FAILED.\r\n");
213
        }
214
}
215

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

    
258
/**
259
 * Causes the robot to join an existing token ring, or create one
260
 * if no token ring exists. The token ring uses global and robot to robot
261
 * packets, and does not rely on any PAN.
262
 **/
263
void wl_token_ring_join()
264
{
265
        WL_DEBUG_PRINT("Joining the token ring.\r\n");
266
        ringState = JOINING;
267
        joinDelay = JOIN_DELAY;
268
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN,
269
                NULL, 0, 0);
270
}
271

    
272
/**
273
 * Causes the robot to leave the token ring. The robot stops
274
 * alerting others of its location, but continues storing the
275
 * locations of other robots.
276
 **/
277
void wl_token_ring_leave()
278
{
279
        ringState = LEAVING;
280
}
281

    
282
/**
283
 * Requests that the specified robot be given the token and
284
 * allowed to flash its BOM. After its BOM is flashed, the
285
 * token will return to the robot who sent it.
286
 *
287
 * @param robot the ID of the robot which should flash its BOM
288
 **/
289
void wl_token_request(int robot)
290
{
291
        char buf[1];
292
        buf[0] = robot;
293
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_REQUEST,
294
                buf, 1, 0);
295
}
296

    
297
/**
298
 * Returns the BOM reading robot source has for robot dest.
299
 *
300
 * @param source the robot that made the BOM reading
301
 * @param dest the robot whose relative location is returned
302
 *
303
 * @return a BOM reading from robot source to robot dest,
304
 * in the range 0-15, or -1 if it is unknown
305
 **/
306
int wl_token_get_sensor_reading(int source, int dest)
307
{
308
        return sensor_matrix_get_reading(sensorMatrix, source, dest);
309
}
310

    
311
/**
312
 * Returns the BOM reading we have for robot dest.
313
 * 
314
 * @param dest the robot whose relative location is returned
315
 *
316
 * @return a BOM reading from us to robot dest, in the range
317
 * 0-15, or -1 if it is unkown
318
 **/
319
int wl_token_get_my_sensor_reading(int dest)
320
{
321
        return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
322
}
323

    
324
/**
325
 * This method is called when we receive a token pass packet.
326
 * @param source is the robot it came from
327
 * @param nextRobot is the robot the token was passed to
328
 * @param sensorData a char with an id followed by a char with the sensor
329
 *                reading for that robot, repeated for sensorDataLength bytes
330
 * @param sensorDataLength the length in bytes of sensorData
331
 */
332
void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength)
333
{
334
        int i, j;
335
        deathDelay = -1;
336

    
337
        WL_DEBUG_PRINT("Received the token, next robot is ");
338
        WL_DEBUG_PRINT_INT((int)nextRobot);
339
        WL_DEBUG_PRINT(" \r\n");
340
        sensor_matrix_set_in_ring(sensorMatrix, source, 1);
341

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

    
378
                        if (i == wl_get_xbee_id() && ringState == MEMBER)
379
                        {
380
                                ringState = NONMEMBER;
381
                                wl_token_ring_join();
382
                                
383
                                WL_DEBUG_PRINT("We have been removed from the ring ");
384
                                WL_DEBUG_PRINT("and are rejoining.\r\n");
385
                        }
386
                        
387
                        //the person who accepted us is dead... let's ask again
388
                        if (i == acceptor)
389
                        {
390
                                sensor_matrix_set_in_ring(sensorMatrix,
391
                                                wl_get_xbee_id(), 1);
392
                                ringState = NONMEMBER;
393
                                acceptor = -1;
394
                                wl_token_ring_join();
395
                        }
396
                }
397
        }
398

    
399
        wl_token_next_robot = nextRobot;
400
        
401
        deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;
402
        
403
        //we have the token
404
        if (wl_token_next_robot == wl_get_xbee_id())
405
                wl_token_get_token();
406
}
407

    
408
/**
409
 * Gets the distance in the token ring between two robots.
410
 *
411
 * @param robot1 the first robot
412
 * @param robot2 the second robot
413
 *
414
 * @return the number of passes before the token is expected
415
 * to reach robot2 from robot1
416
 **/
417
int get_token_distance(int robot1, int robot2)
418
{
419
        int curr = robot1 + 1;
420
        int count = 1;
421
        while (1)
422
        {
423
                if (curr == sensor_matrix_get_size(sensorMatrix))
424
                        curr = 0;
425
                if (curr == robot2)
426
                        break;
427
                if (sensor_matrix_get_in_ring(sensorMatrix, curr))
428
                        count++;
429
                curr++;
430
        }
431
        return count;
432
}
433

    
434
/**
435
 * Passes the token to the next robot in the token ring.
436
 **/
437
void wl_token_pass_token()
438
{
439
        char nextRobot;
440
        int i = wl_get_xbee_id() + 1;
441
        if (accepted == -1)
442
        {
443
                while (1)
444
                {
445
                        if (i == sensor_matrix_get_size(sensorMatrix))
446
                                i = 0;
447
                        if (sensor_matrix_get_in_ring(sensorMatrix, i))
448
                        {
449
                                nextRobot = (char)i;
450
                                break;
451
                        }
452
                        i++;
453
                }
454
        }
455
        else
456
        {
457
                WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");
458
                //add a new robot to the token ring
459
                sensor_matrix_set_in_ring(sensorMatrix, accepted, 1);
460
                nextRobot = accepted;
461
                accepted = -1;
462
        }
463

    
464
        //we don't include ourself
465
        int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
466
        char* buf = (char*)malloc(packetSize * sizeof(char));
467
        if (!buf)
468
        {
469
                WL_DEBUG_PRINT_INT(packetSize);
470
                WL_DEBUG_PRINT("Out of memory - pass token.\r\n");
471
                return;
472
        }
473
        buf[0] = nextRobot;
474

    
475
        int j = 0;
476
        for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
477
                if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
478
                {
479
                        buf[2*j + 1] = i;
480
                        buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);
481
                        j++;
482
                }
483
        
484
        WL_DEBUG_PRINT("Passing the token to robot ");
485
        WL_DEBUG_PRINT_INT(buf[0]);
486
        WL_DEBUG_PRINT(".\r\n");
487
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,
488
                buf, packetSize, 3);
489

    
490
        wl_token_next_robot = nextRobot;
491
        deathDelay = DEATH_DELAY;
492
        free(buf);
493
}
494

    
495
/**
496
 * Called when a packet is received stating that another robot has turned
497
 * its BOM on. Our BOM is then read, and the data is added to the sensor
498
 * matrix.
499
 *
500
 * @param source the robot whose BOM is on
501
 **/
502
void wl_token_bom_on_receive(int source)
503
{
504
        WL_DEBUG_PRINT("Robot ");
505
        WL_DEBUG_PRINT_INT(source);
506
        WL_DEBUG_PRINT(" has flashed its bom.\r\n");
507
        sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(), 
508
                source, get_max_bom_function());
509
}
510

    
511
/**
512
 * This method is called when we receive the token. Upon receiving
513
 * the token, we must send a BOM_ON packet, flash the BOM, and send
514
 * the token to the next robot.
515
 * 
516
 * If there is a pending request for the token, this is processed first.
517
 **/
518
void wl_token_get_token()
519
{
520
        WL_DEBUG_PRINT("We have the token.\r\n");
521
        if (ringState == ACCEPTED)
522
        {
523
                sensor_matrix_set_in_ring(sensorMatrix,
524
                        wl_get_xbee_id(), 1);
525
                WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
526
                ringState = MEMBER;
527
        }
528

    
529
        if (ringState == LEAVING || ringState == NONMEMBER)
530
        {
531
                sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
532
                if (ringState == NONMEMBER)
533
                {
534
                        WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
535
                }
536
                return;
537
        }
538
        
539
        //check for interruption requests
540
        if (queue_size(interrupting) > 0)
541
        {
542
                char buf[1];
543
                buf[0] = (char)(int)queue_remove(interrupting);
544
                
545
                //in case this robot has requested multiple times
546
                queue_remove_all(interrupting, (void*)(int)buf[0]);
547

    
548
                wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_PASS,
549
                        buf, 1, 0);
550

    
551
                deathDelay = DEATH_DELAY;
552
                wl_token_next_robot = buf[0];
553
                return;
554
        }
555

    
556
        WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
557
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,
558
                NULL, 0, 0);
559

    
560
        bom_on_function();
561
        #ifdef ROBOT
562
        delay_ms(BOM_DELAY);
563
        #endif
564
        bom_off_function();
565
        
566
        if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
567
        {
568
                WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
569
        }
570
        
571
        wl_token_pass_token();
572
}
573

    
574
/**
575
 * Called when a request to join the token ring is received.
576
 * If we are the robot preceding the requester in the ring,
577
 * we respond with a JOIN_ACCEPT packet and pass the token to
578
 * this robot when we receive the token.
579
 *
580
 * @param source the robot who requested to join
581
 **/
582
void wl_token_join_receive(int source)
583
{
584
        WL_DEBUG_PRINT("Received joining request from robot ");
585
        WL_DEBUG_PRINT_INT(source);
586
        WL_DEBUG_PRINT(".\r\n");
587

    
588
        //we cannot accept the request if we are not a member
589
        if (ringState != MEMBER)
590
                return;
591
        //if they didn't get our response, see if we should respond again
592
        if (accepted == source)
593
                accepted = -1;
594
        //we can only accept one request at a time
595
        if (accepted != -1)
596
                return;
597
        
598
        //check if we are the preceding robot in the token ring
599
        int i = source - 1;
600
        while (1)
601
        {
602
                if (i < 0)
603
                        i = sensor_matrix_get_size(sensorMatrix) - 1;
604
                //we must send a join acceptance
605
                if (i == wl_get_xbee_id())
606
                        break;
607

    
608
                //another robot will handle it
609
                if (sensor_matrix_get_in_ring(sensorMatrix, i))
610
                        return;
611
                i--;
612
        }
613

    
614
        accepted = source;
615
        wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT,
616
                NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME);
617
        
618
        WL_DEBUG_PRINT("Accepting robot ");
619
        WL_DEBUG_PRINT_INT(source);
620
        WL_DEBUG_PRINT(" into the token ring.\r\n");
621

    
622
        joinDelay = -1;
623
        
624
        // the token ring has not started yet
625
        if (sensor_matrix_get_joined(sensorMatrix) == 1)
626
                wl_token_pass_token();
627
}
628

    
629
/**
630
 * Called when we receive a JOIN_ACCEPT packet in attempting to join
631
 * the token ring.
632
 * Our attempt to join the ring is stopped, and we wait for the token.
633
 *
634
 * @param source the robot who accepted us
635
 **/
636
void wl_token_join_accept_receive(int source)
637
{
638
        WL_DEBUG_PRINT("Accepted into the token ring by robot ");
639
        WL_DEBUG_PRINT_INT(source);
640
        WL_DEBUG_PRINT(".\r\n");
641
        joinDelay = -1;
642
        ringState = ACCEPTED;
643
        acceptor = source;
644

    
645
        //add ourselves to the token ring
646
        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
647
}
648

    
649
/**
650
 * Called when we receive a packet passing the token and interrupting
651
 * the token ring.
652
 * If the token has been passed to us, we flash our BOM
653
 * and pass it back.
654
 *
655
 * @param source the robot who sent the interrupt packet
656
 * @param robot the robot the token has been passed to
657
 **/
658
void wl_token_interrupt_pass_receive(int source, int robot)
659
{
660
        if (wl_get_xbee_id() != robot)
661
        {
662
                queue_remove_all(interrupting, (void*)robot);
663
                wl_token_next_robot = robot;
664
                deathDelay = DEATH_DELAY + rand() / (RAND_MAX / (2 * DEATH_DELAY));
665
                return;
666
        }
667
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,
668
                NULL, 0, 0);
669
        
670
        bom_on_function();
671
        #ifdef ROBOT
672
        delay_ms(BOM_DELAY);
673
        #endif
674
        bom_off_function();
675

    
676
        //we don't include ourself, only if we are in the ring
677
        int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
678
        if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
679
                packetSize += 2;
680
        char* buf = (char*)malloc(packetSize * sizeof(char));
681
        if (!buf)
682
        {
683
                WL_DEBUG_PRINT("Out of memory - pass_receive.\r\n");
684
                return;
685
        }
686
        
687
        //return the token to where we got it from
688
        buf[0] = source;
689

    
690
        int i = 0, j = 0;
691
        for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
692
                if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
693
                {
694
                        buf[2*j + 1] = i;
695
                        buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);
696
                        j++;
697
                }
698
        
699
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,
700
                buf, packetSize, 0);
701

    
702
        wl_token_next_robot = source;
703
        deathDelay = DEATH_DELAY;
704
        free(buf);
705
}
706

    
707
/**
708
 * Called when we receive a request to interrupt the token ring.
709
 * We add the robot to our list of interrupt requests,
710
 * and will send the token to this robot when we next receive the
711
 * token, unless someone else does so first.
712
 *
713
 * @param source the robot requesting interruption
714
 * @param robt the robot requested to interrupt the token ring
715
 **/
716
void wl_token_interrupt_request_receive(int source, int robot)
717
{
718
        queue_add(interrupting, (void*)robot);
719
}
720

    
721
int wl_token_get_num_robots(void){
722
  return sensor_matrix_get_joined(sensorMatrix);
723
}
724

    
725
int wl_token_get_matrix_size(void){
726
  return sensor_matrix_get_size(sensorMatrix);
727
}