root / trunk / code / projects / colonet / utilities / robot_slave / wl_adhoc.c @ 13
History | View | Annotate | Download (18.7 KB)
1 | 13 | emarinel | /**
|
---|---|---|---|
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 | #include <dragonfly_lib.h> |
||
10 | #include "wl_adhoc.h" |
||
11 | #include <avr/interrupt.h> |
||
12 | |||
13 | #include "ring_buffer.h" |
||
14 | #include "bom.h" |
||
15 | |||
16 | #include <colonet_dragonfly.h> |
||
17 | #include <colonet_defs.h> |
||
18 | |||
19 | #define MAX(a, b) ((a) > (b) ? (a) : (b))
|
||
20 | #define MIN(a, b) ((a) < (b) ? (a) : (b))
|
||
21 | |||
22 | //#define USB_DEBUG
|
||
23 | |||
24 | #ifdef USB_DEBUG
|
||
25 | #define USB_PUTC( c ) usb_putc( c )
|
||
26 | #define USB_PUTS( s ) usb_puts( s )
|
||
27 | #define USB_PUTI( i ) usb_puti( i )
|
||
28 | #else
|
||
29 | #define USB_PUTC( c )
|
||
30 | #define USB_PUTS( s )
|
||
31 | #define USB_PUTI( i )
|
||
32 | #endif
|
||
33 | |||
34 | |||
35 | RING_BUFFER_NEW(ring_buffer, 256, char, buffer); |
||
36 | volatile int buffer_size = 0; |
||
37 | |||
38 | void led_user(int x) {} |
||
39 | |||
40 | void usb_putstr(char *s) |
||
41 | { |
||
42 | char *t = s;
|
||
43 | while (*t != 0) |
||
44 | { |
||
45 | usb_putc(*t); |
||
46 | t++; |
||
47 | } |
||
48 | } |
||
49 | |||
50 | unsigned char set_orb_wireless = 1; |
||
51 | unsigned char crazy_debug = 0; |
||
52 | int max_bom = -1; |
||
53 | |||
54 | |||
55 | /*
|
||
56 | \fn wl_timeout_handler
|
||
57 | Handles the timeout case.
|
||
58 | |||
59 | A time out occurs about every 1/4 second. This function is called each time
|
||
60 | one occurs. In the case where a robot first turns on, it will wait for a
|
||
61 | long time before declaring itself the leader. Otherwise, if enough
|
||
62 | time-outs occur in a row, wl_to_flag will be set to 1. This will cause a
|
||
63 | timeout packet to be sent next time around
|
||
64 | |||
65 | */
|
||
66 | void wl_timeout_handler( void ) { |
||
67 | |||
68 | if(crazy_debug) {
|
||
69 | return;
|
||
70 | } |
||
71 | |||
72 | //increment the total number of wireless time-outs
|
||
73 | wl_to_count++; |
||
74 | //lcd_putchar('w');
|
||
75 | USB_PUTC('w');
|
||
76 | |||
77 | static int wl_first_to_count = 0; |
||
78 | |||
79 | //first time time out routine - listen for a long time
|
||
80 | if(first_time) {
|
||
81 | if(wl_to_count > WL_TO_INITIAL){
|
||
82 | //increment the counter for first timeouts
|
||
83 | wl_first_to_count++; |
||
84 | |||
85 | //lcd_putchar('Z');
|
||
86 | |||
87 | //you don't actually want to send out a packet -- you would confuse
|
||
88 | //other robots
|
||
89 | wl_to_flag = 0;
|
||
90 | wl_to_count = 0;
|
||
91 | } |
||
92 | }else{
|
||
93 | //not the first time - regular time out routine
|
||
94 | |||
95 | //
|
||
96 | if ((wl_sent_cs || wl_sent_depart || wl_sent_dock) && wl_to_count > 3) |
||
97 | { |
||
98 | usb_puts("Charging station timed out. \n\r");
|
||
99 | wl_sent_cs = 0;
|
||
100 | wl_sent_depart = 0;
|
||
101 | wl_sent_dock = 0;
|
||
102 | wl_create_packet(); |
||
103 | wl_send(); |
||
104 | } |
||
105 | |||
106 | //if you have enough timeouts
|
||
107 | if(wl_to_count > WL_TIMEOUT){
|
||
108 | //lcd_putchar('z');
|
||
109 | |||
110 | USB_PUTS("|time.out|");
|
||
111 | //change the time-out flag to 1
|
||
112 | wl_to_flag = 1;
|
||
113 | wl_to_count = 0;
|
||
114 | |||
115 | //increment the count for robot death
|
||
116 | if(NUM_BOTS > 1) { |
||
117 | //only increase death count if there are robots that can die
|
||
118 | //if numbots = 1, you are the only robot (no suicidal robots... yet)
|
||
119 | wl_to_death_count++; |
||
120 | } |
||
121 | |||
122 | //check for a robot death
|
||
123 | if(wl_to_death_count > WL_TO_DEATH) {
|
||
124 | //a robot has died :(
|
||
125 | wl_to_death_flag = 1;
|
||
126 | wl_death_handler(); |
||
127 | } |
||
128 | } |
||
129 | } |
||
130 | |||
131 | //take care of the case where you time out so much that you become the leader
|
||
132 | if(wl_first_to_count > WL_TO_LEADER) {
|
||
133 | //you are now the leader
|
||
134 | //lcd_putchar('L');
|
||
135 | |||
136 | first_time = 0;
|
||
137 | |||
138 | NUM_BOTS = 1; //there are (1) robot in the ring |
||
139 | SRC = 0; //you are robot # 1 |
||
140 | DEST = 1; //you are 'speaking' to (nonexistent) robot #1 |
||
141 | |||
142 | /*
|
||
143 | lcd_putchar('[');
|
||
144 | lcd_putchar(SRC+48);
|
||
145 | lcd_putchar('|');
|
||
146 | lcd_putchar(DEST+48);
|
||
147 | lcd_putchar('|');
|
||
148 | lcd_putchar(NUM_BOTS+48);
|
||
149 | lcd_putchar('|');
|
||
150 | lcd_putint(wl_to_death_count);
|
||
151 | lcd_putchar(']');
|
||
152 | */
|
||
153 | usb_putc('t');
|
||
154 | |||
155 | //create a a packet and send it
|
||
156 | wl_create_packet(); |
||
157 | wl_send(); |
||
158 | |||
159 | //change the timeout count to 0
|
||
160 | wl_to_count = 0;
|
||
161 | wl_first_to_count = 0;
|
||
162 | } |
||
163 | } |
||
164 | |||
165 | /**
|
||
166 | \fn wl_error_handler(int errcode)
|
||
167 | Handles error codes.
|
||
168 | \param errcode The error code.
|
||
169 | At the time, the only real error that happens is robot death.
|
||
170 | |||
171 | Robot death requires that wl_buf have been set (either by receiving a
|
||
172 | error packet or by calling wl_death_handler.
|
||
173 | |||
174 | Robot death:
|
||
175 | Number of robots will decrement.
|
||
176 | Robots above the dead robot will decrement their ID and their source.
|
||
177 | The loop remains closed.
|
||
178 | |||
179 | */
|
||
180 | void wl_error_handler(unsigned char errcode) |
||
181 | { |
||
182 | USB_PUTS("Error Code: ");
|
||
183 | USB_PUTI(errcode); |
||
184 | //which robot is dead
|
||
185 | int dead_robot;
|
||
186 | |||
187 | //print debug information
|
||
188 | /*
|
||
189 | lcd_clear_screen();
|
||
190 | lcd_gotoxy(0,0);
|
||
191 | lcd_putstr("error handler ");
|
||
192 | lcd_putint(errcode);
|
||
193 | lcd_gotoxy(0,1);
|
||
194 | */
|
||
195 | |||
196 | USB_PUTS("|error handler|");
|
||
197 | |||
198 | switch(errcode) {
|
||
199 | case WL_ERRCODE_ROBOTDEATH:
|
||
200 | orb_set_color(RED); |
||
201 | buzzer_chirp(1000, C5);
|
||
202 | //lcd_putstr(" dead_robot: ");
|
||
203 | //lcd_putint(wl_buf[WL_ERR_DEAD_LOC]);
|
||
204 | break;
|
||
205 | case WL_ERRCODE_NEEDTOCHARGE:
|
||
206 | //lcd_putstr(" charge_robot: ");
|
||
207 | //lcd_putint(wl_buf[WL_SRC_LOC]);
|
||
208 | break;
|
||
209 | default:
|
||
210 | break;
|
||
211 | } |
||
212 | |||
213 | //lcd_gotoxy(0,2);
|
||
214 | |||
215 | //set all the time-out counts and flags to 0
|
||
216 | wl_to_count = 0;
|
||
217 | wl_to_flag = 0;
|
||
218 | wl_to_death_flag = 0;
|
||
219 | wl_to_death_count = 0;
|
||
220 | |||
221 | //acknowledge receipt of the error packet by setting the errorcode in the
|
||
222 | //packet to 0
|
||
223 | wl_buf[WL_ERROR_LOC] = 0;
|
||
224 | //also set checksum to !0
|
||
225 | wl_chksum = 1;
|
||
226 | |||
227 | //switch the error code
|
||
228 | switch(errcode) {
|
||
229 | //ROBOT DEATH
|
||
230 | case WL_ERRCODE_ROBOTDEATH:
|
||
231 | //get which robot has died
|
||
232 | dead_robot = wl_buf[WL_ERR_DEAD_LOC]; |
||
233 | |||
234 | //decrement the total number of robots
|
||
235 | NUM_BOTS--; |
||
236 | |||
237 | //see if you are above the dead robot
|
||
238 | if( SRC > dead_robot ) {
|
||
239 | //if you are the last robot (since NUM_ROBOTS was decremented,
|
||
240 | //you can check that way) then decrement your number
|
||
241 | //and set your destination to 0
|
||
242 | if( SRC == NUM_BOTS) {
|
||
243 | //lcd_putchar('+');
|
||
244 | //lcd_putchar('1');
|
||
245 | //lcd_putchar('+');
|
||
246 | SRC--; |
||
247 | DEST = 0;
|
||
248 | } else {
|
||
249 | //otherwise decrement both your ID and the destination by 1
|
||
250 | //lcd_putchar('+');
|
||
251 | //lcd_putchar('2');
|
||
252 | //lcd_putchar('+');
|
||
253 | //decrement src and dest
|
||
254 | SRC--; |
||
255 | DEST--; |
||
256 | } |
||
257 | } |
||
258 | |||
259 | //ensure that the last robot talks to the 0th one
|
||
260 | //this won't be set if the LAST robot dies (all robots are < dead)
|
||
261 | if(SRC == (NUM_BOTS - 1) ) { |
||
262 | DEST = 0;
|
||
263 | } |
||
264 | |||
265 | //now your SRC and DEST have been set properly
|
||
266 | //if you are the robot before the dead robot - send a message
|
||
267 | if(SRC == dead_robot) {
|
||
268 | //lcd_putstr(" send packet ");
|
||
269 | //you took the place of the dead robot
|
||
270 | //you start the token ring again
|
||
271 | wl_create_packet(); |
||
272 | wl_send(); |
||
273 | } |
||
274 | break;
|
||
275 | |||
276 | case WL_ERRCODE_NEEDTOCHARGE:
|
||
277 | break; //Do nothing about this, yet |
||
278 | |||
279 | case WL_ERRCODE_CHARGE_ACCEPT:
|
||
280 | // use the value we saved in parse_buffer()
|
||
281 | wl_csloc = max_bom; |
||
282 | USB_PUTS("\n\rBOM direction: ");
|
||
283 | USB_PUTI(wl_csloc); |
||
284 | if (wl_sent_cs)
|
||
285 | { |
||
286 | direction_from_station = wl_buf[WL_DATA_DATA1]; |
||
287 | desired_bay = wl_buf[WL_DATA_DATA2]; |
||
288 | charge_request_register = CHARGE_REQUEST_ACCEPTED; |
||
289 | USB_PUTS("\n\rReceived charge request response\n\r");
|
||
290 | wl_sent_cs = 0;
|
||
291 | wl_create_packet(); |
||
292 | wl_send(); |
||
293 | orb_set_color(BLUE); |
||
294 | } |
||
295 | break;
|
||
296 | |||
297 | case WL_ERRCODE_CHARGE_DENY:
|
||
298 | if (wl_sent_cs)
|
||
299 | { |
||
300 | //TODO: set flag to tell robot to stop moving, etc...
|
||
301 | charge_request_register = CHARGE_REQUEST_NOT_ACCEPTED; |
||
302 | usb_puts("\n\rI HAVE BEEN DENIED\n\r");
|
||
303 | wl_sent_cs = 0;
|
||
304 | wl_create_packet(); |
||
305 | wl_send(); |
||
306 | orb2_set_color(RED); |
||
307 | } |
||
308 | break;
|
||
309 | case WL_ERRCODE_CHARGE_DEPART://we have sent the charging station a depart request, and now need to continue the token ring |
||
310 | if (wl_sent_depart)
|
||
311 | { |
||
312 | usb_puts("Received Departing Ack from Station\n\r");
|
||
313 | wl_sent_depart = 0;
|
||
314 | wl_cs_flag = NOT_REQUEST_CS; |
||
315 | wl_create_packet(); |
||
316 | wl_send(); |
||
317 | } |
||
318 | break;
|
||
319 | case WL_ERRCODE_CHARGE_DOCK:
|
||
320 | if (wl_sent_dock)
|
||
321 | { |
||
322 | usb_puts("Received Docking Ack from Station\n\r");
|
||
323 | wl_sent_dock = 0;
|
||
324 | wl_cs_flag = NOT_REQUEST_CS; |
||
325 | wl_create_packet(); |
||
326 | wl_send(); |
||
327 | } |
||
328 | break;
|
||
329 | default:
|
||
330 | //do other stuff;
|
||
331 | break;
|
||
332 | } |
||
333 | } |
||
334 | |||
335 | /**
|
||
336 | \fn wl_death_handler
|
||
337 | |||
338 | Handles robot death (for the one robot that detects death).
|
||
339 | |||
340 | The robot that currently 'senses' robot death is the robot AFTER the dead
|
||
341 | robot in the ring. It knows that PREV (the robot right before it) is dead.
|
||
342 | Robot creates a error packet and sends it.
|
||
343 | |||
344 | This will also call wl_error_handler since wl_buf was set with the current
|
||
345 | dead robot
|
||
346 | */
|
||
347 | void wl_death_handler( void ) |
||
348 | { |
||
349 | //set time out and death count/flags to 0
|
||
350 | wl_to_count = 0;
|
||
351 | wl_to_flag = 0;
|
||
352 | wl_to_death_flag = 0;
|
||
353 | wl_to_death_count = 0;
|
||
354 | |||
355 | //create a packet with the error
|
||
356 | wl_create_error_packet(WL_ERRCODE_ROBOTDEATH); |
||
357 | wl_buf[WL_ERR_DEAD_LOC] = PREV; //PREV is the robot that is dead
|
||
358 | wl_make_checksum(); //Recompute checksum after adding data
|
||
359 | |||
360 | //send the packet
|
||
361 | wl_send(); |
||
362 | |||
363 | //handle the error yourself
|
||
364 | wl_error_handler(wl_buf[WL_ERROR_LOC]); |
||
365 | |||
366 | return;
|
||
367 | } |
||
368 | |||
369 | /**
|
||
370 | \fun parse_buffer
|
||
371 | Parses the buffer in wl_buf.
|
||
372 | |||
373 | Does nothing if the buffer isn't full.
|
||
374 | */
|
||
375 | void parse_buffer ()
|
||
376 | { |
||
377 | //deal with timeout case
|
||
378 | /*
|
||
379 | if( wl_to_flag ) {
|
||
380 | usb_putc('x');
|
||
381 | wl_to_flag = 0;
|
||
382 | wl_create_packet();
|
||
383 | wl_send();
|
||
384 | return;
|
||
385 | }
|
||
386 | */
|
||
387 | while (1) |
||
388 | { |
||
389 | if (buffer_size < WL_PKT_LEN)
|
||
390 | return;
|
||
391 | |||
392 | char c;
|
||
393 | RING_BUFFER_REMOVE(buffer, c); |
||
394 | buffer_size--; |
||
395 | //if (usb_debug)
|
||
396 | // usb_putc(c);
|
||
397 | if (c == 'R') |
||
398 | { |
||
399 | RING_BUFFER_REMOVE(buffer, c); |
||
400 | //fprintf(usb_fd, "%x ", c);
|
||
401 | buffer_size--; |
||
402 | //if (usb_debug)
|
||
403 | // usb_putc(c);
|
||
404 | if (c == 'C') |
||
405 | break;
|
||
406 | } |
||
407 | } |
||
408 | //wl_to_count = 0;
|
||
409 | wl_index = 2;
|
||
410 | wl_chksum = 0;
|
||
411 | |||
412 | //read bom here, save value for later (error_handler??)
|
||
413 | //max_bom = getMaxBom();
|
||
414 | |||
415 | //get the packet from the ring buffer -- make sure it's valid
|
||
416 | while (wl_index != 0) |
||
417 | { |
||
418 | char wl_input;
|
||
419 | RING_BUFFER_REMOVE(buffer, wl_input); |
||
420 | buffer_size--; |
||
421 | |||
422 | if( wl_index >= WL_SRC_LOC ) {
|
||
423 | if( wl_index < WL_CHK_LOC ) {
|
||
424 | |||
425 | //getting the rest of the packet
|
||
426 | wl_buf[wl_index] = wl_input; |
||
427 | wl_chksum += wl_input; |
||
428 | wl_index++; |
||
429 | |||
430 | }else{
|
||
431 | //we are at checksum location
|
||
432 | wl_buf[wl_index] = 0;
|
||
433 | wl_chksum += wl_input; |
||
434 | wl_index = 0;
|
||
435 | |||
436 | USB_PUTC('o');
|
||
437 | |||
438 | if(!wl_chksum) {
|
||
439 | //checksum is correct - equal to zero
|
||
440 | if(set_orb_wireless) {
|
||
441 | //orb2_set_color(GREEN);
|
||
442 | } |
||
443 | //lcd_putchar('p');
|
||
444 | |||
445 | USB_PUTC('p');
|
||
446 | |||
447 | //get the maximum BOM from the person who sent you the packet
|
||
448 | //max_bom = getMaxBom();
|
||
449 | //lcd_putint(max_bom);
|
||
450 | //lcd_putchar(' ');
|
||
451 | //add to your own sensor the other robot's direction
|
||
452 | |||
453 | // max_bom you them
|
||
454 | //usb_puti(max_bom);
|
||
455 | //usb_putc(' ');
|
||
456 | //addSensor(max_bom, SRC, wl_buf[WL_SRC_LOC], NUM_BOTS);
|
||
457 | |||
458 | //also set whether that robot can see the light or not
|
||
459 | //set_light_status(wl_buf[WL_SRC_LOC], wl_buf[WL_DATA_LIGHT_LOC]);
|
||
460 | }else{
|
||
461 | //chksum failed
|
||
462 | //usb_putc('F');
|
||
463 | int i;
|
||
464 | for(i = 0; i < WL_CHK_LOC; i++) { |
||
465 | USB_PUTC(wl_buf[i]); |
||
466 | } |
||
467 | USB_PUTS("Fail");
|
||
468 | usb_putstr("Fail");
|
||
469 | return;
|
||
470 | //need to return -- was at the end of the ISR
|
||
471 | } |
||
472 | break;
|
||
473 | } |
||
474 | } |
||
475 | } |
||
476 | |||
477 | //acknowledge you take the packet by changing the chksum to nonzero
|
||
478 | wl_chksum = 1;
|
||
479 | |||
480 | USB_PUTS("|received_a_packet|");
|
||
481 | |||
482 | if (wl_buf[5] == COLONET_REQUEST || wl_buf[5] == COLONET_COMMAND) |
||
483 | { |
||
484 | //orb_set_color(YELLOW);
|
||
485 | //colonet_handle_message(SRC, wl_buf);
|
||
486 | colonet_handle_message(wl_buf); |
||
487 | return;
|
||
488 | } |
||
489 | |||
490 | //this is the case when you get your first packet -- you want to join the ring
|
||
491 | if(first_time == 1) { |
||
492 | |||
493 | USB_PUTS("|first_packet|");
|
||
494 | |||
495 | //you received a first packet from someone
|
||
496 | first_time = 0;
|
||
497 | |||
498 | //get the current number of robots
|
||
499 | NUM_BOTS = wl_buf[NUM_BOTS_LOC]; |
||
500 | |||
501 | //you are now the last robot in the line
|
||
502 | SRC = NUM_BOTS; |
||
503 | //you are sending back to the first robot
|
||
504 | DEST = 0;
|
||
505 | |||
506 | //since you've just added yourself, increase numbots
|
||
507 | NUM_BOTS++; |
||
508 | |||
509 | //wl_wait_for_turn = 1
|
||
510 | |||
511 | //figure out a way to send out a packet TODO
|
||
512 | wl_create_packet(); |
||
513 | wl_send(); |
||
514 | } |
||
515 | |||
516 | if(wl_wait_for_turn) {
|
||
517 | if(NUM_BOTS <= 2 || wl_buf[WL_DEST_LOC] == SRC - 1 ) { |
||
518 | //you just got a packet from the 'old' last robot send yours
|
||
519 | //lcd_putchar('S');
|
||
520 | wl_wait_for_turn = 0;
|
||
521 | first_time = 1;
|
||
522 | //so we don't wait with a delay
|
||
523 | wl_create_packet(); |
||
524 | wl_send(); |
||
525 | first_time = 0;
|
||
526 | return;
|
||
527 | }else{
|
||
528 | //otherwise just exit out of parse
|
||
529 | return;
|
||
530 | } |
||
531 | } |
||
532 | |||
533 | //check for error packet first
|
||
534 | if(wl_buf[WL_DEST_LOC] == WL_PKT_ERROR){
|
||
535 | orb2_set_color(RED); |
||
536 | //handle error packet
|
||
537 | wl_error_handler( wl_buf[WL_ERROR_LOC] ); |
||
538 | return;
|
||
539 | } |
||
540 | |||
541 | //see if there are more robots this turn around -- NUM_ROBOTS would have
|
||
542 | //increased
|
||
543 | if(wl_buf[NUM_BOTS_LOC] > NUM_BOTS){
|
||
544 | //if you are the last robot
|
||
545 | if(DEST == 0){ |
||
546 | //if you are the last robot in the ring
|
||
547 | //change your dest to the new number (the last bot)
|
||
548 | DEST = NUM_BOTS; |
||
549 | } |
||
550 | |||
551 | NUM_BOTS = wl_buf[NUM_BOTS_LOC]; |
||
552 | } |
||
553 | |||
554 | if(wl_buf[WL_SRC_LOC] == SRC) {
|
||
555 | return;
|
||
556 | } |
||
557 | |||
558 | //increment time out count so we respond first if we are second in line
|
||
559 | int next = wl_buf[WL_DEST_LOC] + 1; |
||
560 | if (next == NUM_BOTS)
|
||
561 | next = 0;
|
||
562 | if (next == SRC)
|
||
563 | wl_to_count = 1;
|
||
564 | |||
565 | //get the data out of the sensor row
|
||
566 | int i = 0; |
||
567 | int self = wl_buf[WL_SRC_LOC];
|
||
568 | //13-4 = 9
|
||
569 | int end = MIN(WL_DATA_ROW_END, NUM_BOTS+WL_DATA_ROW_START);
|
||
570 | |||
571 | for(i = WL_DATA_ROW_START; i < end; i++){
|
||
572 | addSensor(wl_buf[i], self, (i-WL_DATA_ROW_START), NUM_BOTS); |
||
573 | } |
||
574 | |||
575 | //if you are the destination - respond
|
||
576 | if(wl_buf[WL_DEST_LOC] == SRC) {
|
||
577 | |||
578 | //you know that you got a message addressed to you -- previous robot must
|
||
579 | //not be dead
|
||
580 | wl_to_death_count = 0;
|
||
581 | //the robot previous to you is in wl_buf[WL_SRC_LOC]
|
||
582 | PREV = wl_buf[WL_SRC_LOC]; |
||
583 | |||
584 | //create a packet
|
||
585 | //lcd_putchar('s');
|
||
586 | USB_PUTS("|received.packet.addr.me|");
|
||
587 | |||
588 | switch (wl_cs_flag)
|
||
589 | { |
||
590 | case REQUEST_CS:
|
||
591 | wl_sent_cs = 1;
|
||
592 | wl_create_charging_error_packet(); |
||
593 | wl_send(); |
||
594 | usb_puts("\n\rRequested charge\n\r");
|
||
595 | return;
|
||
596 | case DEPART_CS:
|
||
597 | wl_sent_depart = 1;
|
||
598 | wl_create_leaving_packet(); |
||
599 | wl_send(); |
||
600 | //wl_cs_flag = NOT_REQUEST_CS;
|
||
601 | usb_puts("\n\rSent Leaving Packet\n\r");
|
||
602 | return;
|
||
603 | case ARRIVE_CS:
|
||
604 | wl_sent_dock = 1;
|
||
605 | wl_create_docking_packet(); |
||
606 | wl_send(); |
||
607 | //wl_cs_flag = NOT_REQUEST_CS;
|
||
608 | usb_puts("Sent Docking Packet\n\r");
|
||
609 | return;
|
||
610 | } |
||
611 | |||
612 | wl_create_packet(); |
||
613 | wl_send(); |
||
614 | } |
||
615 | else
|
||
616 | { |
||
617 | usb_putstr("packet not addressed to me");
|
||
618 | usb_puti(wl_buf[WL_DEST_LOC]); |
||
619 | } |
||
620 | |||
621 | return;
|
||
622 | } |
||
623 | |||
624 | /**
|
||
625 | \fun wl_init
|
||
626 | */
|
||
627 | void wl_init(void) |
||
628 | { |
||
629 | wl_index = 0;
|
||
630 | wl_chksum = 1;
|
||
631 | wl_buf[0] = 'R'; |
||
632 | wl_buf[1] = 'C'; |
||
633 | |||
634 | /* executes the function about every 1 sec */
|
||
635 | //rtc_init(HALF_SECOND, &wl_timeout_handler);
|
||
636 | |||
637 | wl_to_flag = 0;
|
||
638 | wl_to_count = 0;
|
||
639 | wl_to_death_count = 0;
|
||
640 | wl_to_death_flag = 0;
|
||
641 | |||
642 | wl_to_max = WL_TO_INITIAL; |
||
643 | first_time = 1;
|
||
644 | wl_wait_for_turn = 0;
|
||
645 | wl_sent_cs = 0;
|
||
646 | wl_sent_depart = 0;
|
||
647 | wl_sent_dock = 0;
|
||
648 | |||
649 | UCSR1B |= _BV(RXCIE); |
||
650 | |||
651 | sei(); |
||
652 | } |
||
653 | |||
654 | void wl_send ( void ) { |
||
655 | int i = 0; |
||
656 | |||
657 | //turn off interrupt so you don't receive packet as you send it
|
||
658 | UCSR1B &= ~_BV(RXCIE); |
||
659 | |||
660 | |||
661 | USB_PUTS("\n\r|sending---\n\r");
|
||
662 | |||
663 | if(set_orb_wireless) {
|
||
664 | orb2_set_color(BLUE); |
||
665 | } |
||
666 | |||
667 | led_user(1);
|
||
668 | |||
669 | if(SRC == (NUM_BOTS - 1)) { |
||
670 | //if you are the last robot
|
||
671 | //delay for some time
|
||
672 | if(set_orb_wireless) {
|
||
673 | orb2_set_color(RED); |
||
674 | } |
||
675 | delay_ms(LAST_ROBOT_DELAY); |
||
676 | } |
||
677 | |||
678 | bom_on(); |
||
679 | delay_ms(WL_DELAY_BOM_PRE); |
||
680 | for(i = 0; i < WL_PKT_LEN; i++) { |
||
681 | //print characters to wireless
|
||
682 | xbee_putc(wl_buf[i]); |
||
683 | |||
684 | //print characters to usb if needed
|
||
685 | //USB_PUTC(wl_buf[i]);
|
||
686 | } |
||
687 | |||
688 | delay_ms(WL_DELAY_BOM_POST); |
||
689 | //this is where you would turn the BOM off
|
||
690 | bom_off(); |
||
691 | |||
692 | led_user(0);
|
||
693 | //sprintf(buf, "%d\n\r", wl_buf[WL_CHK_LOC]);
|
||
694 | //put_string(buf);
|
||
695 | |||
696 | //turn it back on so you can get things now
|
||
697 | UCSR1B |= _BV(RXCIE); |
||
698 | } |
||
699 | |||
700 | void wl_create_packet () {
|
||
701 | //Header for Colony Project
|
||
702 | wl_buf[0] = 'R'; |
||
703 | wl_buf[1] = 'C'; |
||
704 | |||
705 | wl_buf[WL_SRC_LOC] = SRC; //Where the packet came from
|
||
706 | wl_buf[WL_DEST_LOC] = DEST; //Who the packet is for
|
||
707 | |||
708 | //Behavior Specific Data goes here
|
||
709 | wl_buf[WL_DATA_DATA0] = 0;
|
||
710 | wl_buf[WL_DATA_DATA1] = 0;
|
||
711 | wl_buf[WL_DATA_DATA2] = 0;
|
||
712 | |||
713 | //fill packet with data -- sensor row
|
||
714 | for(int i = WL_DATA_ROW_START; i <= WL_DATA_ROW_END; i++) { |
||
715 | wl_buf[i] = sensors[SRC][i - WL_DATA_ROW_START]; |
||
716 | } |
||
717 | |||
718 | //add the number of robots in there
|
||
719 | wl_buf[NUM_BOTS_LOC] = NUM_BOTS; |
||
720 | |||
721 | //create the checksum
|
||
722 | wl_make_checksum(); |
||
723 | } |
||
724 | |||
725 | //Add custom data to the packets
|
||
726 | void wl_create_data_packet(unsigned char d0, unsigned char d1, |
||
727 | unsigned char d2) |
||
728 | { |
||
729 | //Header for Colony Project
|
||
730 | wl_buf[0] = 'R'; |
||
731 | wl_buf[1] = 'C'; |
||
732 | |||
733 | wl_buf[WL_SRC_LOC] = SRC; //Where the packet came from
|
||
734 | wl_buf[WL_DEST_LOC] = DEST; //Who the packet is for
|
||
735 | |||
736 | //Behavior Specific Data goes here
|
||
737 | wl_buf[WL_DATA_DATA0] = d0; |
||
738 | wl_buf[WL_DATA_DATA1] = d1; |
||
739 | wl_buf[WL_DATA_DATA2] = d2; |
||
740 | |||
741 | //fill packet with data -- sensor row
|
||
742 | for(int i = WL_DATA_ROW_START; i <= WL_DATA_ROW_END; i++) { |
||
743 | wl_buf[i] = sensors[SRC][i - WL_DATA_ROW_START]; |
||
744 | } |
||
745 | |||
746 | //add the number of robots in there
|
||
747 | wl_buf[NUM_BOTS_LOC] = NUM_BOTS; |
||
748 | |||
749 | //create the checksum
|
||
750 | wl_make_checksum(); |
||
751 | } |
||
752 | |||
753 | void wl_create_leaving_packet()
|
||
754 | { |
||
755 | wl_create_error_packet(WL_ERRCODE_CHARGE_DEPART); |
||
756 | } |
||
757 | |||
758 | void wl_create_docking_packet()
|
||
759 | { |
||
760 | wl_create_error_packet(WL_ERRCODE_CHARGE_DOCK); |
||
761 | } |
||
762 | |||
763 | void wl_create_charging_error_packet()
|
||
764 | { |
||
765 | wl_create_error_packet(WL_ERRCODE_NEEDTOCHARGE); |
||
766 | } |
||
767 | |||
768 | //Send an error packet
|
||
769 | void wl_create_error_packet(unsigned char errcode) |
||
770 | { |
||
771 | //Header for Colony Project
|
||
772 | wl_buf[0] = 'R'; |
||
773 | wl_buf[1] = 'C'; |
||
774 | |||
775 | //DEST is all robots
|
||
776 | wl_buf[WL_SRC_LOC] = SRC; |
||
777 | wl_buf[WL_DEST_LOC] = WL_PKT_ERROR; |
||
778 | wl_buf[WL_ERROR_LOC] = errcode; |
||
779 | |||
780 | //rest of the packet is just a bunch of 0s
|
||
781 | for (int i = WL_ERR_DEAD_LOC + 1; i <= WL_DATA_END; i++) { |
||
782 | wl_buf[i] = 0;
|
||
783 | } |
||
784 | |||
785 | //add the number of robots in there
|
||
786 | wl_buf[NUM_BOTS_LOC] = NUM_BOTS; |
||
787 | |||
788 | //create the checksum
|
||
789 | wl_make_checksum(); |
||
790 | } |
||
791 | |||
792 | //Computes the checksum
|
||
793 | void wl_make_checksum()
|
||
794 | { |
||
795 | char checksum = 0; |
||
796 | |||
797 | for(int i = WL_SRC_LOC; i < WL_CHK_LOC; i++) { |
||
798 | checksum -= wl_buf[i]; |
||
799 | } |
||
800 | |||
801 | wl_buf[WL_CHK_LOC] = checksum; |
||
802 | } |
||
803 | |||
804 | ISR(USART1_RX_vect) { |
||
805 | char tmp = UDR1;
|
||
806 | RING_BUFFER_ADD(buffer, tmp); |
||
807 | buffer_size++; |
||
808 | |||
809 | //we got a byte, so we're not timing out
|
||
810 | wl_to_count = 0;
|
||
811 | |||
812 | return;
|
||
813 | } |