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.
trunk/code/projects/traffic_navigation/traffic_navigation.h | ||
---|---|---|
3 | 3 |
#include <dragonfly_lib.h> |
4 | 4 |
#include <wl_basic.h> |
5 | 5 |
#include "lineDrive.h" |
6 |
#include "highways.h" |
|
6 | 7 |
|
7 | 8 |
/*States*/ |
8 | 9 |
#define SROAD 0 |
... | ... | |
56 | 57 |
*/ |
57 | 58 |
#define ISPING(p) ((p)[0]==WPINGGLOBAL || (p)[0]==WPINGBOT || (p)[0]==WPINGQUEUE) |
58 | 59 |
|
59 |
#endif |
|
60 |
#endif |
trunk/code/projects/traffic_navigation/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 |
*/ |
trunk/code/projects/traffic_navigation/main.c | ||
---|---|---|
208 | 208 |
} |
209 | 209 |
break; |
210 | 210 |
case SHIGHWAY:/*On highway*/ |
211 |
driveHighway(); |
|
212 |
break; |
|
211 |
highwayFSM(); |
|
213 | 212 |
default: |
214 | 213 |
usb_puts("I got stuck in an unknown state! My state is "); |
215 | 214 |
usb_puti(state); |
trunk/code/projects/traffic_navigation/highways.h | ||
---|---|---|
4 | 4 |
void driveHighway(void); |
5 | 5 |
|
6 | 6 |
|
7 |
#define COUNT_MAX 5
|
|
8 |
#define PASS_DISTANCE 100
|
|
7 |
#define COUNT_MAX 8
|
|
8 |
#define PASS_DISTANCE 150
|
|
9 | 9 |
#endif |
Also available in: Unified diff