root / trunk / code / projects / traffic_navigation / highways.c @ 1891
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 |
*/
|
21 |
int range = range_read_distance(IR2);
|
22 |
usb_puti(range); |
23 |
usb_puts(" : ");
|
24 |
usb_puti(averagei); |
25 |
usb_puts("\r\n");
|
26 |
orb1_set_color(PURPLE); |
27 |
if(range == -1){ |
28 |
range = 200;
|
29 |
} |
30 |
counti ++; |
31 |
averagei += range; |
32 |
if(counti>= COUNT_MAX){
|
33 |
if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)
|
34 |
states = 1;
|
35 |
averagei = 0;
|
36 |
counti = 0;
|
37 |
} |
38 |
break;
|
39 |
} |
40 |
case 1: |
41 |
merge(ILEFT); |
42 |
states = 2;
|
43 |
canMerge = 0;
|
44 |
case 2: |
45 |
if(doDrive(160) == FINISHED) states = 0; |
46 |
orb1_set_color(BLUE); |
47 |
break;
|
48 |
} |
49 |
return 0; |
50 |
} |
51 |
|
52 |
/*
|
53 |
void driveHighway( void ){
|
54 |
doDrive(200);
|
55 |
int range = range_read_distance(IR2);
|
56 |
usb_puti(range);
|
57 |
usb_puts(" : ");
|
58 |
usb_puti(average);
|
59 |
usb_puts("\r\n");
|
60 |
if(range == -1){
|
61 |
return;
|
62 |
}
|
63 |
|
64 |
count ++;
|
65 |
average += range;
|
66 |
if(count >= COUNT_MAX){
|
67 |
if(state[0] != MERGELEFT){
|
68 |
orb1_set_color(ORANGE);
|
69 |
if(average / COUNT_MAX < PASS_DISTANCE){
|
70 |
merge(ILEFT);
|
71 |
usb_puts("MERGE \r\n");
|
72 |
}
|
73 |
}
|
74 |
|
75 |
count =0;
|
76 |
average = 0;
|
77 |
}
|
78 |
//else orb1_set_color(GREEN);
|
79 |
}
|
80 |
*/
|