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