Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (20.8 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
//current robot to check in the iterator
74
int iteratorCount = 0;
75

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

    
88
PacketGroupHandler wl_token_ring_handler =
89
                {WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
90
                wl_token_ring_response_handler, wl_token_ring_receive_handler,
91
                wl_token_ring_cleanup};
92

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

    
114
        wl_register_packet_group(&wl_token_ring_handler);
115
}
116

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

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

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

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

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

    
199
        if (deathDelay >= 0)
200
                deathDelay--;
201
        if (joinDelay >= 0)
202
                joinDelay--;
203
}
204

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    
493
        wl_token_next_robot = nextRobot;
494
        deathDelay = DEATH_DELAY;
495
        free(buf);
496
}
497

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

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

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

    
551
                wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_PASS,
552
                        buf, 1, 0);
553

    
554
                deathDelay = DEATH_DELAY;
555
                wl_token_next_robot = buf[0];
556
                return;
557
        }
558

    
559
        WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
560
        wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,
561
                NULL, 0, 0);
562

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

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

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

    
611
                //another robot will handle it
612
                if (sensor_matrix_get_in_ring(sensorMatrix, i))
613
                        return;
614
                i--;
615
        }
616

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

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

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

    
648
        //add ourselves to the token ring
649
        sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
650
}
651

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

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

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

    
705
        wl_token_next_robot = source;
706
        deathDelay = DEATH_DELAY;
707
        free(buf);
708
}
709

    
710
/**
711
 * Returns the number of robots in the token ring.
712
 *
713
 * @return the number of robots in the token ring
714
 **/
715
int wl_token_get_robots_in_ring(void)
716
{
717
        return sensor_matrix_get_joined(sensorMatrix);
718
}
719

    
720
/**
721
 * Returns true if the specified robot is in the token ring, false
722
 * otherwise.
723
 *
724
 * @param robot the robot to check for whether it is in the token ring
725
 * @return nonzero if the robot is in the token ring, zero otherwise
726
 **/
727
int wl_token_is_robot_in_ring(int robot)
728
{
729
        return sensor_matrix_get_in_ring(sensorMatrix, robot);
730
}
731

    
732
/**
733
 * Begins iterating through the robots in the token ring.
734
 *
735
 * @see wl_token_iterator_has_next, wl_token_iterator_next
736
 **/
737
void wl_token_iterator_begin(void)
738
{
739
        int i;
740
        iteratorCount = 0;
741
        while (!sensor_matrix_get_in_ring(sensorMatrix, i) &&
742
                        i < sensor_matrix_get_size(sensorMatrix))
743
                i++;
744
        if (i == sensor_matrix_get_size(sensorMatrix))
745
                i = -1;
746
}
747

    
748
/**
749
 * Returns true if there are more robots in the token ring
750
 * to iterate through, and false otherwise.
751
 *
752
 * @return nonzero if there are more robots to iterate through,
753
 * zero otherwise
754
 *
755
 * @see wl_token_iterator_begin, wl_token_iterator_next
756
 **/
757
int wl_token_iterator_has_next(void)
758
{
759
        return iteratorCount == -1;
760
}
761

    
762
/**
763
 * Returns the next robot ID in the token ring.
764
 *
765
 * @return the next robot ID in the token ring, or -1 if none exists
766
 *
767
 * @see wl_token_iterator_begin, wl_token_iterator_has_next
768
 **/
769
int wl_token_iterator_next(void)
770
{
771
        int result = iteratorCount;
772
        if (result < 0)
773
                return result;
774

    
775
        iteratorCount++;
776
        while (!sensor_matrix_get_in_ring(sensorMatrix, iteratorCount) &&
777
                iteratorCount < sensor_matrix_get_size(sensorMatrix))
778
                iteratorCount++;
779
        if (iteratorCount == sensor_matrix_get_size(sensorMatrix))
780
                iteratorCount = -1;
781
        return result;
782
}
783

    
784
/**
785
 * Called when we receive a request to interrupt the token ring.
786
 * We add the robot to our list of interrupt requests,
787
 * and will send the token to this robot when we next receive the
788
 * token, unless someone else does so first.
789
 *
790
 * @param source the robot requesting interruption
791
 * @param robt the robot requested to interrupt the token ring
792
 **/
793
void wl_token_interrupt_request_receive(int source, int robot)
794
{
795
        queue_add(interrupting, (void*)robot);
796
}
797

    
798
int wl_token_get_num_robots(void){
799
  return sensor_matrix_get_joined(sensorMatrix);
800
}
801

    
802
int wl_token_get_matrix_size(void){
803
  return sensor_matrix_get_size(sensorMatrix);
804
}