root / trunk / code / projects / traffic_navigation / highways.c @ 1876
History | View | Annotate | Download (618 Bytes)
1 | 1876 | markwill | #include "highways.h" |
---|---|---|---|
2 | #include "lineDrive.h" |
||
3 | #include <dragonfly_lib.h> |
||
4 | extern int state[5]; |
||
5 | static int count =0; |
||
6 | static long average = 0; |
||
7 | |||
8 | void driveHighway( void ){ |
||
9 | doDrive(200);
|
||
10 | int range = range_read_distance(IR2);
|
||
11 | usb_puti(range); |
||
12 | usb_puts(" : ");
|
||
13 | usb_puti(average); |
||
14 | usb_puts("\r\n");
|
||
15 | if(range == -1){ |
||
16 | return;
|
||
17 | } |
||
18 | |||
19 | count ++; |
||
20 | average += range; |
||
21 | if(count >= COUNT_MAX){
|
||
22 | if(state[0] != MERGELEFT){ |
||
23 | orb1_set_color(ORANGE); |
||
24 | if(average / COUNT_MAX < PASS_DISTANCE){
|
||
25 | merge(ILEFT); |
||
26 | usb_puts("MERGE \r\n");
|
||
27 | } |
||
28 | } |
||
29 | count =0;
|
||
30 | average = 0;
|
||
31 | } |
||
32 | //else orb1_set_color(GREEN);
|
||
33 | } |