root / trunk / code / projects / linefollowing / lineDrive.c @ 1855
History | View | Annotate | Download (2.24 KB)
1 |
#include "lineDrive.h" |
---|---|
2 |
|
3 |
int state[5]; |
4 |
int stateCounter;
|
5 |
int stateLength;
|
6 |
int stopped=1; |
7 |
|
8 |
|
9 |
|
10 |
void lineDrive_init()
|
11 |
{ |
12 |
lineFollow_init(); |
13 |
for(int i=0; i<5; i++)state[i]=0; |
14 |
stateCounter=0;
|
15 |
stateLength=0;
|
16 |
stopped=0;
|
17 |
} |
18 |
|
19 |
|
20 |
|
21 |
int doDrive(int speed) |
22 |
{ |
23 |
if(stopped)return NORMAL; |
24 |
|
25 |
int code;
|
26 |
switch(state[0]) |
27 |
{ |
28 |
case ISTRAIGHT:
|
29 |
code = lineFollow(speed); |
30 |
if(code==INTERSECTION)
|
31 |
{ |
32 |
for(int i=0; i<4; i++) state[i]=state[i+1]; |
33 |
state[4]=0; |
34 |
if(state[0]==0)stateCounter++; |
35 |
break;
|
36 |
} |
37 |
else if(code==NOBARCODE) return NORMAL; |
38 |
return code;
|
39 |
|
40 |
|
41 |
case ILEFT:
|
42 |
code = turnLeft(); |
43 |
if(code==0) |
44 |
{ |
45 |
state[0]=0; |
46 |
stateCounter++; |
47 |
} |
48 |
break;
|
49 |
|
50 |
case IRIGHT:
|
51 |
code = turnRight(); |
52 |
if(code==0) |
53 |
{ |
54 |
state[0]=0; |
55 |
stateCounter++; |
56 |
} |
57 |
break;
|
58 |
|
59 |
|
60 |
case MERGELEFT:
|
61 |
code = mergeLeft(); |
62 |
if(code==0) |
63 |
{ |
64 |
state[0]=0; |
65 |
stateLength=0;
|
66 |
return FINISHED;
|
67 |
} |
68 |
return NORMAL;
|
69 |
|
70 |
case MERGERIGHT:
|
71 |
code = mergeRight(); |
72 |
if(code==0) |
73 |
{ |
74 |
state[0]=0; |
75 |
stateLength=0;
|
76 |
return FINISHED;
|
77 |
} |
78 |
return NORMAL;
|
79 |
|
80 |
default:
|
81 |
return LOST;
|
82 |
|
83 |
} |
84 |
|
85 |
if(stateCounter>=stateLength)
|
86 |
{ |
87 |
stateCounter=stateLength=0;
|
88 |
return FINISHED;
|
89 |
} |
90 |
return NORMAL;
|
91 |
} |
92 |
|
93 |
|
94 |
void start(void){stopped=0;} |
95 |
void stop(void){stopped=1;} |
96 |
|
97 |
|
98 |
int merge(int dir) |
99 |
{ |
100 |
if(stateLength!=0)return ERROR; |
101 |
stateLength++; |
102 |
state[0]=(dir==ILEFT ? MERGELEFT : MERGERIGHT);
|
103 |
return NORMAL;
|
104 |
} |
105 |
|
106 |
int turn(int type, int dir) |
107 |
{ |
108 |
if(stateLength!=0)return ERROR; |
109 |
if(dir==IRIGHT){stateLength++; state[1]=IRIGHT; return NORMAL;} |
110 |
if(dir==IUTURN){stateLength+=2; state[1]=state[2]=ILEFT; return NORMAL;} |
111 |
if(dir==ISTRAIGHT && type==SINGLE){stateLength++; state[1]=ISTRAIGHT; return NORMAL;} |
112 |
if(dir==ISTRAIGHT){stateLength+=2; state[1]=state[2]=ISTRAIGHT; return NORMAL;} |
113 |
//must be left turn
|
114 |
if(type==SINGLE){stateLength++; state[1]=ILEFT; return NORMAL;} |
115 |
if(type==DOUBLE){stateLength+=3;state[1]=state[3]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;} |
116 |
if(type==ON_RAMP){stateLength+=2; state[1]=ILEFT; state[2]=ISTRAIGHT; return NORMAL;} |
117 |
if(type==OFF_RAMP){stateLength+=2; state[1]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;} |
118 |
|
119 |
//Should never get here
|
120 |
return ERROR;
|
121 |
} |
122 |
|