Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / colonet / testing / robot_routine_reg_test / wl_adhoc.c @ 13

History | View | Annotate | Download (20 KB)

1
/**
2
   \file wl_adhoc.c
3
   Wireless ad hoc networking.
4
   NOW WITH SENSORS
5
   Robots can come in and out of the ring.
6
*/
7
 
8
/*
9
  Wireless ad hoc networking + sensors
10

11
  If a robot is the first to turn on, it will set itself as the leader
12

13
  The next robot will receive that packet and tokenize with it.
14
  Each other robot turned on will receive packets, and join the ring.
15
  All the robots will also change their network topology (num_bots)
16
  
17
  
18
  2006-11-21: Felix
19
              Fixed buzzer.c code (copied from a different dir).
20
              Code still works on robots at this time 
21
              
22
              Felix used this code to figure out the robot resetting problem.  BOM sensor 10
23
              needs to be taped up.
24
              You may put this event in the history books.
25
              
26
  
27
  2006-10-31 Kevin
28
  Changed the boards to the 115200 Baud rate. Changed the pound define to reflect this.
29
  The boards have been hardcoded to do this. They are labelled in case you do not know
30
  which ones do this. It works.
31
  
32
  2006-10-13: Kevin
33
  Made improvements to make the module more modular and general
34
  so that you can just drag and drop it into any behavior.
35
  Began the integration of autonomous charging. We're using the
36
  error system that is built into wl_adhoc originally to ping the
37
  charge stations. Do not know if it works, can't test it yet. But
38
  wireless normally still works.
39

40
  2006-08-07: Felix
41
  Added the use of the sensor row that was being transmitted
42
  Robots now share a common table of sensor 'positions'
43
  Just add light sensing and you're good to go
44
            
45
  WARNING: Robots seem to reset randomly due to batteries, especially
46
  when moving around.  Could be brownout detector kicking in.
47
  Added a beep at startup to detect reset.
48

49
  2006-08-03: Felix
50
  Now added follow-the-leader code
51
  Battery problem -- testing only really worked on power supply
52
            
53
  Robots follow the leader (theoretically).
54
  They fill their own row of the sensors matrix, but do not get
55
  the other robots' row -- not in the code.
56

57
  2006-07-27: Felix
58
  Created new directory for sensors use
59

60
  2006-07-24: Felix
61
  Added BOM.c/BOM.h
62
  bom_test works -- gets max bom
63
            
64
  Added the get_maxbom each time you get a wireless packet
65
  ---NEEDS TESTING
66
            
67
  Also, packet sent should contain your 'row'
68
  Also, received packet should be stored in matrix
69

70
  2006-07-13: Felix
71
  Works for the most part
72
  W00t
73
                        
74
  Still a few kinds I imagine, but robot death & birth work
75
*/
76

    
77
#include "firefly+_lib.h"
78
#include "wl_adhoc.h"
79
#include <avr/interrupt.h>
80
#include "colonet.h"
81
#include "colonet_defs.h"
82

    
83
#define MAX(a, b) ((a) > (b) ? (a) : (b))
84
#define MIN(a, b)  ((a) < (b) ? (a) : (b))
85

    
86
unsigned char set_orb_wireless = 0;
87
//unsigned char set_orb_wireless = 1;
88

    
89
/*
90
  int main( void ) 
91
  {
92
  wl_adhoc();
93
  bom_test();
94

95
  return 0;
96
  }
97
*/
98

    
99
void bom_test (void)
100
{
101
  int maxb;
102

    
103
  lcd_init();
104
  analog_init();
105

    
106
  while(1) {
107
    if(button1_read()){
108
      lcd_clear_screen();
109
      bom_on();
110
      delay_ms(100);
111
    }
112
    
113
    if(button2_read()) {
114
      lcd_clear_screen();
115
      bom_off();
116
      delay_ms(100);
117
    }
118
        
119
    maxb = getMaxBom();
120
        
121
    lcd_putint(maxb);
122

    
123
    delay_ms(50); 
124
  }
125
}
126

    
127
void wl_adhoc()
128
{
129
  //initialize the serial modules
130
  serial_init(WL_BAUD);       //XBee
131
  serial1_init(BAUD115200);   //Serial port
132
  
133
  //lcd, orb, all that good stuff
134
  lcd_init();
135
  analog_init();
136
  led_init();
137
  orb_init();
138
  motors_init();
139
  buzzer_init();
140
        
141
  //init the wireless
142
  wl_init();
143
        
144
  //int max_bom_to_follow;
145

    
146
  while(1){
147
    //this needs to happen continuously -- it handles the packet
148
    parse_buffer();
149
        
150
    /*
151
      if(SRC ==  0) {
152
      //leader -- just drive
153
      drive_forward();
154
      }
155
      else {
156
      max_bom_to_follow = getSelfSensor(SRC, PREV, NUM_BOTS);
157
      //lcd_putchar('f');
158
      //lcd_putchar(max_bom_to_follow + 48);
159
      //lcd_putchar(' ');
160
      seek(max_bom_to_follow);
161
      }
162
    */
163
                
164
    //button1 prints debug information
165
    if(button1_read()) {
166
      //lcd_clear_screen();
167
      display_debug();
168
      delay_ms(10);
169
    }
170
                
171
    /*
172
    //button2 does nothing
173
    if(button2_read()) {
174
      //wl_create_packet();
175
      //wl_send();
176
      //delay_ms(10);
177
    }
178
    */
179
  }
180

    
181
  return;        
182
}
183

    
184
/**
185
   \fn display_debug
186
   \brief displays debug information
187

188
   Prints out 4 numbers, in the following order
189
   ( PREV | SRC | DEST | NUM_BOTS | wl_to_death_count )
190
   where PREV is the previous robot before you
191
   SRC is you
192
   DEST is your destination
193
   NUM_BOTS is the total number of robots in the ring
194
   wl_do_death_count is the number of timeouts that will result in a death
195
*/
196
void display_debug()
197
{
198
  lcd_clear_screen();
199
  lcd_putchar('(');
200
  lcd_putchar(PREV+48);
201
  lcd_putchar('|');
202
  lcd_putchar(SRC+48);
203
  lcd_putchar('|');
204
  lcd_putchar(DEST+48);
205
  lcd_putchar('|');
206
  lcd_putchar(NUM_BOTS+48);        
207
  lcd_putchar('|');
208
  lcd_putint(wl_to_death_count);
209
  lcd_putchar(')');
210
    
211
  //print table
212
  int y = 1;
213
  lcd_gotoxy(0, y);
214
  int i, j;
215

    
216
  for(i = 0; i < NUM_BOTS; i++) {
217
    for(j = 0; j < NUM_BOTS; j++) {
218
      lcd_putchar( getSelfSensor(i, j, NUM_BOTS) + 48);
219
      lcd_putchar(' ');
220
    }
221
    y++;
222
    lcd_gotoxy(0, y);
223
  }
224
}
225

    
226
/*
227
  \fn wl_timeout_handler
228
  Handles the timeout case.
229

230
  A time out occurs about every 1/4 second.  This function is called each time 
231
  one occurs.  In the case where a robot first turns on, it will wait for a 
232
  long time before declaring itself the leader.  Otherwise, if enough 
233
  time-outs occur in a row, wl_to_flag will be set to 1.  This will cause a 
234
  timeout packet to be sent next time around
235

236
*/
237
void wl_timeout_handler( void ) {
238
  //increment the total number of wireless time-outs
239
  wl_to_count++;
240
  //lcd_putchar('w');
241
        
242
  static int wl_first_to_count =  0;
243
        
244
  //first time time out routine - listen for a long time
245
  if(first_time) {
246
    if(wl_to_count > WL_TO_INITIAL){
247
      //increment the counter for first timeouts
248
      wl_first_to_count++;
249
                                
250
      //lcd_putchar('Z');
251
                                
252
      //you don't actually want to send out a packet -- you would confuse 
253
      //other robots
254
      wl_to_flag = 0;
255
      wl_to_count = 0;
256
    }
257
  }else{
258
    //not the first time - regular time out routine
259

    
260
    //if you have enough timeouts
261
    if(wl_to_count > WL_TIMEOUT){
262
      //lcd_putchar('z');
263
                        
264
      //change the time-out flag to 1
265
      wl_to_flag = 1;
266
      wl_to_count = 0;
267
                        
268
      //increment the count for robot death
269
      if(NUM_BOTS > 1) {
270
        //only increase death count if there are robots that can die
271
        //if numbots = 1, you are the only robot (no suicidal robots... yet)
272
        wl_to_death_count++;
273
      }
274
      
275
      //check for a robot death
276
      if(wl_to_death_count > WL_TO_DEATH) {
277
        //a robot has died :(
278
        wl_to_death_flag = 1;
279
        wl_death_handler();
280
      }
281
    }
282
  }
283
        
284
  //take care of the case where you time out so much that you become the leader
285
  if(wl_first_to_count > WL_TO_LEADER) {
286
    //you are now the leader
287
    //lcd_putchar('L');
288
                
289
    first_time = 0;
290
                                
291
    NUM_BOTS = 1;                        //there are (1) robot in the ring
292
    SRC = 0;                                //you are robot # 1
293
    DEST = 1;                                //you are 'speaking' to (nonexistent) robot #1
294
                
295
    lcd_putchar('[');
296
    lcd_putchar(SRC+48);
297
    lcd_putchar('|');
298
    lcd_putchar(DEST+48);
299
    lcd_putchar('|');
300
    lcd_putchar(NUM_BOTS+48);        
301
    lcd_putchar('|');
302
    lcd_putint(wl_to_death_count);
303
    lcd_putchar(']');
304
                
305
    //create a a packet and send it
306
    wl_create_packet();
307
    wl_send();
308
                
309
    //change the timeout count to 0
310
    wl_to_count = 0;
311
    wl_first_to_count = 0;
312
  }
313
}
314

    
315
/**
316
   \fn wl_error_handler(int errcode)
317
   Handles error codes.
318
   \param errcode The error code.
319
   At the time, the only real error that happens is robot death.
320

321
   Robot death requires that wl_buf have been set (either by receiving a
322
   error packet or by calling wl_death_handler.
323

324
   Robot death:
325
   Number of robots will decrement.
326
   Robots above the dead robot will decrement their ID and their source.
327
   The loop remains closed.
328

329
*/
330
void wl_error_handler(unsigned char errcode)
331
{
332
  //which robot is dead
333
  int dead_robot;
334

    
335
  //print debug information
336
  lcd_clear_screen();
337
  lcd_gotoxy(0,0);
338
  lcd_putstr("error handler    ");
339
  lcd_putint(errcode);
340
  lcd_gotoxy(0,1);
341

    
342
  switch(errcode) {
343
  case WL_ERRCODE_ROBOTDEATH:
344
    lcd_putstr("  dead_robot: ");
345
    lcd_putint(wl_buf[WL_ERR_DEAD_LOC]);
346
    break;
347
  case WL_ERRCODE_NEEDTOCHARGE:
348
    lcd_putstr("   charge_robot: ");
349
    lcd_putint(wl_buf[WL_SRC_LOC]);
350
    break;
351
  default:
352
    break;
353
  }
354

    
355
  lcd_gotoxy(0,2);
356
        
357
  //set all the time-out counts and flags to 0
358
  wl_to_count = 0;
359
  wl_to_flag = 0;
360
  wl_to_death_flag = 0;
361
  wl_to_death_count = 0;
362
                
363
  //acknowledge receipt of the error packet by setting the errorcode in the 
364
  //packet to 0
365
  wl_buf[WL_ERROR_LOC] = 0;
366
  //also set checksum to !0
367
  wl_chksum = 1;
368
        
369
  //switch the error code
370
  switch(errcode) {
371
    //ROBOT DEATH
372
  case WL_ERRCODE_ROBOTDEATH:
373
    //get which robot has died
374
    dead_robot = wl_buf[WL_ERR_DEAD_LOC];
375
                        
376
    //decrement the total number of robots
377
    NUM_BOTS--;
378
                        
379
    //see if you are above the dead robot
380
    if( SRC > dead_robot ) {
381
      //if you are the last robot (since NUM_ROBOTS was decremented, 
382
      //you can check that way) then decrement your number
383
      //and set your destination to 0
384
      if( SRC == NUM_BOTS) {
385
        lcd_putchar('+');
386
        lcd_putchar('1');
387
        lcd_putchar('+');
388
        SRC--;
389
        DEST = 0;
390
      } else {
391
        //otherwise decrement both your ID and the destination by 1
392
        lcd_putchar('+');
393
        lcd_putchar('2');
394
        lcd_putchar('+');
395
        //decrement src and dest
396
        SRC--;
397
        DEST--;
398
      }
399
    }
400
                        
401
    //ensure that the last robot talks to the 0th one
402
    //this won't be set if the LAST robot dies (all robots are < dead)
403
    if(SRC == (NUM_BOTS - 1) ) {
404
      DEST = 0;
405
    }
406
    
407
    //now your SRC and DEST have been set properly
408
    //if you are the robot before the dead robot - send a message
409
    if(SRC == dead_robot) {
410
      lcd_putstr(" send packet ");
411
      //you took the place of the dead robot
412
      //you start the token ring again
413
      wl_create_packet();
414
      wl_send();
415
    }
416
    break;
417
      
418
  case WL_ERRCODE_NEEDTOCHARGE:
419
    if (CHARGESTATION == 1) {
420
      /*Turn on BOM for the length of time robots normally would have their 
421
        BOM on. This is effectively telling everyone where the charging 
422
        station is.  Assuming only one charging station at the moment.
423
      */
424
      bom_on();
425
      delay_ms(WL_DELAY_BOM_PRE);
426
      wl_create_error_packet(WL_ERRCODE_CHARGESTATION);
427
      wl_send();
428
      delay_ms(WL_DELAY_BOM_POST);
429
      bom_off();
430
    }
431
    break;      //Do nothing about this, yet
432
        
433
  case WL_ERRCODE_CHARGESTATION:
434
    // If the Charging Station responds, look for where it's comming from and 
435
    //store that value for later
436
    //wl_csloc = getMaxBOM();
437
    break;
438
      
439
  default:
440
    //do other stuff;
441
    break;
442
  }        
443
}
444

    
445
/**
446
   \fn wl_death_handler
447

448
   Handles robot death (for the one robot that detects death).
449

450
   The robot that currently 'senses' robot death is the robot AFTER the dead
451
   robot in the ring.  It knows that PREV (the robot right before it) is dead.
452
   Robot creates a error packet and sends it. 
453

454
   This will also call wl_error_handler since wl_buf was set with the current
455
   dead robot
456
*/
457
void wl_death_handler( void ) 
458
{
459
  //set time out and death count/flags to 0
460
  wl_to_count = 0;
461
  wl_to_flag = 0;
462
  wl_to_death_flag = 0;
463
  wl_to_death_count = 0;
464

    
465
  //create a packet with the error
466
  wl_create_error_packet(WL_ERRCODE_ROBOTDEATH);
467
  wl_buf[WL_ERR_DEAD_LOC] = PREV;   //PREV is the robot that is dead
468
  wl_make_checksum();   //Recompute checksum after adding data
469
        
470
  //send the packet
471
  wl_send();
472
        
473
  //handle the error yourself
474
  wl_error_handler(wl_buf[WL_ERROR_LOC]);
475
        
476
  return;
477
}
478

    
479
/**
480
   \fun parse_buffer
481
   Parses the buffer in wl_buf.
482

483
   Does nothing if the buffer isn't full.
484
*/
485
void parse_buffer(void) 
486
{
487
  //deal with timeout case
488
  if( wl_to_flag ) {
489
    wl_to_flag = 0;
490
    wl_create_packet();
491
    wl_send();
492
    return;
493
  }
494

    
495
  //do nothing is packet is not complete
496
  if( (wl_chksum != 0) || (wl_index != 0) ) {
497
    return;
498
  }
499
  
500
  //acknowledge you take the packet by changing the chksum to nonzero
501
  wl_chksum = 1;
502

    
503
  if(wl_buf[WL_DATA_DATA1] == COLONET_REQUEST || 
504
     wl_buf[WL_DATA_DATA1] == COLONET_COMMAND){
505
    printf("colonet message received: code %d\n", wl_buf[WL_DATA_DATA2]);
506
    colonet_handle_message(SRC, wl_buf);
507
  }else{
508
    printf("\ndata1:%d\n", wl_buf[WL_DATA_DATA1]);
509
  }
510
        
511
  if(first_time == 1) {
512
    //you received a first packet from someone
513
    first_time = 0;
514
                
515
    //get the current number of robots
516
    NUM_BOTS = wl_buf[NUM_BOTS_LOC];
517
                
518
    //you are now the last robot in the line
519
    SRC = NUM_BOTS;
520
    //you are sending back to the first robot
521
    DEST = 0;
522
                
523
    //since you've just added yourself, increase numbots
524
    NUM_BOTS++;
525
                
526
    //figure out a way to send out a packet TODO
527
    //wl_wait_for_turn = 1;
528
                
529
    //create and send a packet
530
    wl_create_packet();
531
    wl_send();
532
  }
533
  
534
  //IS NOT CALLED FOR NOW
535
  if(wl_wait_for_turn) {
536
    if( wl_buf[WL_DEST_LOC] == (NUM_BOTS - 1) ) {
537
      //you just got a packet from the 'old' last robot send yours
538
      //lcd_putchar('S');
539
      wl_create_packet();
540
      wl_send();
541
    }else{
542
      //otherwise just exit out of parse
543
      return;
544
    }
545
  }
546
  
547
  //check for error packet first
548
  if(wl_buf[WL_DEST_LOC] == WL_PKT_ERROR){
549
    //handle error packet
550
    wl_error_handler( wl_buf[WL_ERROR_LOC] );
551
    return;
552
  }
553
  
554
  //see if there are more robots this turn around -- NUM_ROBOTS would have 
555
  //increased
556
  if(wl_buf[NUM_BOTS_LOC] > NUM_BOTS){
557
    //if you are the last robot
558
    if(DEST == 0){
559
      //if you are the last robot in the ring
560
      //change your dest to the new number (the last bot)
561
      DEST = NUM_BOTS;
562
      //lcd_putchar('j');
563
    }
564

    
565
    //increase the number of robots to what it is in the incoming packet
566
    NUM_BOTS = wl_buf[NUM_BOTS_LOC];
567
  }
568
                
569
  //make sure packet isn't your own (shouldn't really happen)
570
  if(wl_buf[WL_SRC_LOC] == SRC) {
571
    //lcd_putchar('d');
572
    return;
573
  }
574
  
575
  //get the data out of the sensor row
576
  int i = 0;
577
  int self = wl_buf[WL_SRC_LOC];
578
  //13-4 = 9
579
  int end = MIN(WL_DATA_ROW_END, NUM_BOTS+WL_DATA_ROW_START);
580
  //lcd_putchar('s');
581
  //lcd_putchar(self+48);
582
  //lcd_putchar(':');
583
  //lcd_putchar(end+48);
584
  for(i = WL_DATA_ROW_START; i < end; i++){
585
    addSensor(wl_buf[i], self, (i-WL_DATA_ROW_START), NUM_BOTS);
586
  }
587
  
588
  //if you are the destination - respond
589
  if(wl_buf[WL_DEST_LOC] == SRC) {
590
    //you know that you got a message addressed to you -- previous robot must 
591
    //not be dead
592
    wl_to_death_count = 0;
593
    //the robot previous to you is in wl_buf[WL_SRC_LOC]
594
    PREV = wl_buf[WL_SRC_LOC];
595
    
596
    //create a packet
597
    //lcd_putchar('s');
598
    wl_create_packet();
599
    wl_send();
600
  }
601

    
602
  return;
603
}
604

    
605
/**
606
   \fun wl_init
607
*/
608
void wl_init(void)
609
{
610
  lcd_putchar('~');
611
  lcd_putchar(SRC+48);
612
  lcd_putchar('~');
613
  lcd_putchar(DEST+48);
614
  lcd_putchar('~');
615
  lcd_putchar(NUM_BOTS+48);
616
  lcd_putchar('~');
617
    
618
  buzzer_init();
619
  buzzer_set_freq(C5);
620
  delay_ms(800);
621
  buzzer_off();
622
        
623
  wl_index = 0;
624
  wl_chksum = 1;
625
  wl_buf[0] = 'R';
626
  wl_buf[1] = 'C';
627
        
628
  /* executes the function about every 1/4 sec */
629
  rtc_init(PRESCALE_DIV_128, 64, &wl_timeout_handler); 
630

    
631
  wl_to_flag = 0;
632
  wl_to_count = 0; 
633
  wl_to_death_count = 0;
634
  wl_to_death_flag = 0;
635
        
636
  wl_to_max = WL_TO_INITIAL;
637
  first_time = 1;
638
  wl_wait_for_turn = 0;
639
        
640
  UCSR0B |= _BV(RXCIE) | _BV(RXEN);
641
        
642
  sei();
643
}
644

    
645
void wl_send ( void ) {
646
  int i = 0;
647
  
648
  if(set_orb_wireless) {
649
    orb_set_color(BLUE);
650
  }
651
  
652
  led_user(1);
653
  
654
  if(SRC == (NUM_BOTS - 1) ) {
655
    //if you are the last robot
656
    //delay for some time
657
    if(set_orb_wireless) {
658
      orb_set_color(RED);
659
    }
660
    delay_ms(LAST_ROBOT_DELAY);
661
    
662
    //warning: delay is actually 0 -- adding the delay back causes issues.
663
  }
664
  
665
  //this is where you would turn on the BOM
666
  bom_on();
667
  delay_ms(WL_DELAY_BOM_PRE);
668
  for(i = 0; i < WL_PKT_LEN; i++) {
669
    serial_putchar(wl_buf[i]);
670
    serial1_putchar(wl_buf[i]);
671
  }
672
  
673
  delay_ms(WL_DELAY_BOM_POST);
674
  //this is where you would turn the BOM off
675
  bom_off();
676
  
677
  led_user(0);
678
  //sprintf(buf, "%d\n\r", wl_buf[WL_CHK_LOC]);
679
  //put_string(buf);
680
}
681

    
682
void wl_create_packet() {
683
  //Header for Colony Project
684
  wl_buf[0] = 'R';
685
  wl_buf[1] = 'C';
686
  
687
  wl_buf[WL_SRC_LOC] = SRC;     //Where the packet came from
688
  wl_buf[WL_DEST_LOC] = DEST;   //Who the packet is for
689
  
690
  //Behavior Specific Data goes here
691
  wl_buf[WL_DATA_DATA0] = 0;
692
  wl_buf[WL_DATA_DATA1] = 0;
693
  wl_buf[WL_DATA_DATA2] = 0;
694
  
695
  //fill packet with data -- sensor row
696
  for(int i = WL_DATA_ROW_START; i <= WL_DATA_ROW_END; i++) {
697
    wl_buf[i] = sensors[SRC][i - WL_DATA_ROW_START];
698
  }
699
  
700
  //add the number of robots in there
701
  wl_buf[NUM_BOTS_LOC] = NUM_BOTS;
702
  
703
  //create the checksum
704
  wl_make_checksum();
705
}
706

    
707
//Add custom data to the packets
708
void wl_create_data_packet(unsigned char d0, unsigned char d1, 
709
                           unsigned char d2)
710
{
711
  //Header for Colony Project
712
  wl_buf[0] = 'R';
713
  wl_buf[1] = 'C';
714
        
715
  wl_buf[WL_SRC_LOC] = SRC;     //Where the packet came from
716
  wl_buf[WL_DEST_LOC] = DEST;   //Who the packet is for
717
    
718
  //Behavior Specific Data goes here
719
  wl_buf[WL_DATA_DATA0] = d0;
720
  wl_buf[WL_DATA_DATA1] = d1;
721
  wl_buf[WL_DATA_DATA2] = d2;
722
        
723
  //fill packet with data -- sensor row
724
  for(int i = WL_DATA_ROW_START; i <= WL_DATA_ROW_END; i++) {
725
    wl_buf[i] = sensors[SRC][i - WL_DATA_ROW_START];
726
  }
727
        
728
  //add the number of robots in there
729
  wl_buf[NUM_BOTS_LOC] = NUM_BOTS;
730
  
731
  //create the checksum
732
  wl_make_checksum();
733
}
734

    
735
//Send an error packet
736
void wl_create_error_packet(unsigned char errcode)
737
{
738
  //Header for Colony Project
739
  wl_buf[0] = 'R';
740
  wl_buf[1] = 'C';
741
  
742
  //DEST is all robots
743
  wl_buf[WL_SRC_LOC] = SRC;
744
  wl_buf[WL_DEST_LOC] = WL_PKT_ERROR;
745
  wl_buf[WL_ERROR_LOC] = errcode;
746
        
747
  //rest of the packet is just a bunch of 0s
748
  for(int i = WL_ERR_DEAD_LOC + 1; i <= WL_DATA_END; i++) {
749
    wl_buf[i] = 0;
750
  }
751
        
752
  //add the number of robots in there
753
  wl_buf[NUM_BOTS_LOC] = NUM_BOTS;
754
        
755
  //create the checksum
756
  wl_make_checksum();
757
}
758

    
759
//Computes the checksum
760
void wl_make_checksum() 
761
{
762
  char checksum = 0;
763

    
764
  for(int i = WL_SRC_LOC; i < WL_CHK_LOC; i++) {
765
    checksum -= wl_buf[i];
766
  }
767

    
768
  wl_buf[WL_CHK_LOC] = checksum;
769
}
770

    
771
//use on new avr-libc
772
//ISR(USART0_RX_vect)
773
SIGNAL(SIG_USART0_RECV)
774
{
775
  int max_bom;
776
  
777
  wl_input = UDR0;
778
  
779
  //printf("%d ", wl_input);
780
        
781
  if((wl_index == 0) && (wl_chksum != 0)){
782
    //first packet? - check chksum to make sure we're not beginning
783
    if( wl_input == 'R' ) {
784
      //got the first code
785
      wl_index++;
786
      
787
      //if(set_orb_wireless) {
788
      //  orb_set_color(GREEN);
789
      //}
790
      //lcd_putchar('R');
791
      //serial1_putchar('R');
792
                        
793
      wl_to_count = 0;                   //at this point we're not timing out, reset counter
794
    }
795
    return;
796
  }else if( wl_index == 1 ){
797
    if(wl_input == 'C') {
798
      wl_index++;
799
      wl_chksum = 0;
800
      //lcd_putchar('C');
801
      //serial1_putchar('C');
802
    }else{
803
      //incomplete packet header
804
      wl_index = 0;
805
    }
806
    return;
807
  }
808
        
809
  if( wl_index >= WL_SRC_LOC ) {
810
    if( wl_index < WL_CHK_LOC ) {
811
      //getting the rest of the packet
812
      wl_buf[wl_index] = wl_input;
813
      wl_chksum += wl_input;
814
      wl_index++;
815
    }else{
816
      //we are at checksum location
817
      wl_buf[wl_index] = 0;
818
      wl_chksum += wl_input;
819
      wl_index = 0;
820
      if(!wl_chksum) {
821
        //checksum is correct - equal to zero
822
        //if(set_orb_wireless) {
823
        //  orb_set_color(ORB_OFF);
824
        //}
825
        //lcd_putchar('p');
826
                
827
        //get the maximum BOM from the person who sent you the packet
828
        max_bom = getMaxBom();
829
        //lcd_putint(max_bom);
830
        //lcd_putchar(' ');
831
        //add to your own sensor the other robot's direction
832
                
833
        //        max_bom  you  them
834
        addSensor(max_bom, SRC, wl_buf[WL_SRC_LOC], NUM_BOTS);
835
                
836
        //also set whether that robot can see the light or not
837
        //set_light_status(wl_buf[WL_SRC_LOC], wl_buf[WL_DATA_LIGHT_LOC]);
838
      }else{  
839
        //chksum failed
840
        //serial1_putchar('F');
841
        //lcd_putchar('F');
842
      }
843
    }
844
  }
845
}