Revision 1877
added highways.h to the traffic_navigation.h file, made highways.c/h actually work for merging left. changed main.c to run the new highways.c code.
highways.c | ||
---|---|---|
1 | 1 |
#include "highways.h" |
2 | 2 |
#include "lineDrive.h" |
3 | 3 |
#include <dragonfly_lib.h> |
4 |
extern int state[5]; |
|
5 |
static int count =0; |
|
6 |
static long average = 0; |
|
4 |
static int counti =0; |
|
5 |
static long averagei = 0; |
|
6 |
static int states = 0; |
|
7 |
int canMerge = 1; |
|
7 | 8 |
|
9 |
void highwayFSM(){ |
|
10 |
switch(states){ |
|
11 |
|
|
12 |
case 0: // Normal Drive |
|
13 |
doDrive(225); |
|
14 |
int range = range_read_distance(IR2); |
|
15 |
usb_puti(range); |
|
16 |
usb_puts(" : "); |
|
17 |
usb_puti(averagei); |
|
18 |
usb_puts("\r\n"); |
|
19 |
orb1_set_color(PURPLE); |
|
20 |
if(range == -1){ |
|
21 |
return; |
|
22 |
} |
|
23 |
|
|
24 |
counti ++; |
|
25 |
averagei += range; |
|
26 |
if(counti>= COUNT_MAX){ |
|
27 |
if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge) |
|
28 |
states = 1; |
|
29 |
averagei = 0; |
|
30 |
counti = 0; |
|
31 |
} |
|
32 |
break; |
|
33 |
case 1: |
|
34 |
merge(ILEFT); |
|
35 |
states = 2; |
|
36 |
canMerge = 0; |
|
37 |
case 2: |
|
38 |
if(doDrive(160) == FINISHED) states = 0; |
|
39 |
orb1_set_color(BLUE); |
|
40 |
break; |
|
41 |
} |
|
42 |
} |
|
43 |
|
|
44 |
/* |
|
8 | 45 |
void driveHighway( void ){ |
9 | 46 |
doDrive(200); |
10 | 47 |
int range = range_read_distance(IR2); |
... | ... | |
26 | 63 |
usb_puts("MERGE \r\n"); |
27 | 64 |
} |
28 | 65 |
} |
66 |
|
|
29 | 67 |
count =0; |
30 | 68 |
average = 0; |
31 | 69 |
} |
32 | 70 |
//else orb1_set_color(GREEN); |
33 | 71 |
} |
34 |
|
|
72 |
*/ |
Also available in: Unified diff