root / trunk / code / projects / traffic_navigation / highways.c @ 1890
History | View | Annotate | Download (1.41 KB)
1 |
#include "highways.h" |
---|---|
2 |
#include "lineDrive.h" |
3 |
#include <dragonfly_lib.h> |
4 |
|
5 |
static int counti =0; |
6 |
static long averagei = 0; |
7 |
static int states = 0; |
8 |
int canMerge = 1; |
9 |
void highwayStart(){
|
10 |
canMerge = 1;
|
11 |
} |
12 |
int highwayFSM(){
|
13 |
switch(states){
|
14 |
|
15 |
case 0: // Normal Drive |
16 |
{ |
17 |
int theCode = doDrive(225); |
18 |
if(theCode != NORMAL && theCode >= 0) |
19 |
return theCode;
|
20 |
int range = range_read_distance(IR2);
|
21 |
usb_puti(range); |
22 |
usb_puts(" : ");
|
23 |
usb_puti(averagei); |
24 |
usb_puts("\r\n");
|
25 |
orb1_set_color(PURPLE); |
26 |
if(range == -1){ |
27 |
range = 200;
|
28 |
} |
29 |
counti ++; |
30 |
averagei += range; |
31 |
if(counti>= COUNT_MAX){
|
32 |
if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)
|
33 |
states = 1;
|
34 |
averagei = 0;
|
35 |
counti = 0;
|
36 |
} |
37 |
break;
|
38 |
} |
39 |
case 1: |
40 |
merge(ILEFT); |
41 |
states = 2;
|
42 |
canMerge = 0;
|
43 |
case 2: |
44 |
if(doDrive(160) == FINISHED) states = 0; |
45 |
orb1_set_color(BLUE); |
46 |
break;
|
47 |
} |
48 |
return 0; |
49 |
} |
50 |
|
51 |
/*
|
52 |
void driveHighway( void ){
|
53 |
doDrive(200);
|
54 |
int range = range_read_distance(IR2);
|
55 |
usb_puti(range);
|
56 |
usb_puts(" : ");
|
57 |
usb_puti(average);
|
58 |
usb_puts("\r\n");
|
59 |
if(range == -1){
|
60 |
return;
|
61 |
}
|
62 |
|
63 |
count ++;
|
64 |
average += range;
|
65 |
if(count >= COUNT_MAX){
|
66 |
if(state[0] != MERGELEFT){
|
67 |
orb1_set_color(ORANGE);
|
68 |
if(average / COUNT_MAX < PASS_DISTANCE){
|
69 |
merge(ILEFT);
|
70 |
usb_puts("MERGE \r\n");
|
71 |
}
|
72 |
}
|
73 |
|
74 |
count =0;
|
75 |
average = 0;
|
76 |
}
|
77 |
//else orb1_set_color(GREEN);
|
78 |
}
|
79 |
*/
|