root / trunk / code / projects / traffic_navigation / highways.c @ 1899
History | View | Annotate | Download (1.77 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 |
|
10 |
void highwayStart(){
|
11 |
canMerge = 1;
|
12 |
} |
13 |
int changeLanes(){
|
14 |
if(counti<10000){ |
15 |
counti++; |
16 |
motor_r_set(FORWARD,232);
|
17 |
motor_l_set(FORWARD,200);
|
18 |
return 0; |
19 |
} |
20 |
int colours[5]; |
21 |
updateLine(colours); |
22 |
int pos = lineLocate(colours);
|
23 |
if(pos >= 3 || pos <= -3){ |
24 |
motor_r_set(FORWARD,187);
|
25 |
motor_l_set(FORWARD,190);
|
26 |
return 0; |
27 |
} |
28 |
counti = 0;
|
29 |
return 1; |
30 |
} |
31 |
int highwayFSM(){
|
32 |
switch(states){
|
33 |
|
34 |
case 0: // Normal Drive |
35 |
{ |
36 |
int theCode = doDrive(205); |
37 |
/* if(theCode != NORMAL && theCode >= 0)
|
38 |
return theCode;
|
39 |
*/
|
40 |
int range = range_read_distance(IR2);
|
41 |
usb_puti(range); |
42 |
usb_puts(" : ");
|
43 |
usb_puti(averagei); |
44 |
usb_puts("\r\n");
|
45 |
orb1_set_color(CYAN); |
46 |
if(range == -1){ |
47 |
range = 200;
|
48 |
} |
49 |
counti ++; |
50 |
averagei += range; |
51 |
if(counti>= COUNT_MAX){
|
52 |
if(averagei / COUNT_MAX < PASS_DISTANCE && canMerge)
|
53 |
states = 1;
|
54 |
averagei = 0;
|
55 |
counti = 0;
|
56 |
} |
57 |
break;
|
58 |
} |
59 |
case 1: |
60 |
//merge(ILEFT);
|
61 |
states = 2;
|
62 |
canMerge = 0;
|
63 |
case 2: |
64 |
if(changeLanes()){
|
65 |
states = 0;
|
66 |
orb1_set_color(GREEN); |
67 |
} |
68 |
else orb1_set_color(YELLOW);
|
69 |
break;
|
70 |
} |
71 |
return 0; |
72 |
} |
73 |
|
74 |
/*
|
75 |
void driveHighway( void ){
|
76 |
doDrive(200);
|
77 |
int range = range_read_distance(IR2);
|
78 |
usb_puti(range);
|
79 |
usb_puts(" : ");
|
80 |
usb_puti(average);
|
81 |
usb_puts("\r\n");
|
82 |
if(range == -1){
|
83 |
return;
|
84 |
}
|
85 |
|
86 |
count ++;
|
87 |
average += range;
|
88 |
if(count >= COUNT_MAX){
|
89 |
if(state[0] != MERGELEFT){
|
90 |
orb1_set_color(ORANGE);
|
91 |
if(average / COUNT_MAX < PASS_DISTANCE){
|
92 |
merge(ILEFT);
|
93 |
usb_puts("MERGE \r\n");
|
94 |
}
|
95 |
}
|
96 |
|
97 |
count =0;
|
98 |
average = 0;
|
99 |
}
|
100 |
//else orb1_set_color(GREEN);
|
101 |
}
|
102 |
*/
|