root / trunk / code / projects / activities_fair_demo / lineDrive.c @ 2001
History | View | Annotate | Download (3.83 KB)
1 |
/**
|
---|---|
2 |
* @file lineDrive.c
|
3 |
*
|
4 |
* Provides functions to implement line driving behavior. This program extends
|
5 |
* the behavior of the line-following program by following lines automatically
|
6 |
* and implementing behaviors to deal with commands passed to lineDrive.
|
7 |
*
|
8 |
* @author Dan Jacobs
|
9 |
* @date 11-1-2010
|
10 |
*/
|
11 |
|
12 |
#include "lineDrive.h" |
13 |
|
14 |
int state[5]; //! Stores a queue of sub-commands to be executed |
15 |
int stateCounter;
|
16 |
int stateLength;
|
17 |
|
18 |
//! Whether lineDrive is currently paused. Set to 0 on initialization.
|
19 |
int stopped=1; |
20 |
|
21 |
|
22 |
/**
|
23 |
* Starts the line following procedure. Must be called before other
|
24 |
* line-following functions will work. This function essentially resets the
|
25 |
* state of line-following.
|
26 |
*/
|
27 |
void lineDrive_init()
|
28 |
{ |
29 |
lineFollow_init(); |
30 |
for(int i=0; i<5; i++)state[i]=0; |
31 |
stateCounter=0;
|
32 |
stateLength=0;
|
33 |
stopped=0;
|
34 |
} |
35 |
|
36 |
|
37 |
/**
|
38 |
* Follows a line and executes whatever command is next on the queue.
|
39 |
* @param speed The speed with which to drive along the line.
|
40 |
*/
|
41 |
int doDrive(int speed) |
42 |
{ |
43 |
if(stopped)
|
44 |
{ |
45 |
motor_l_set(FORWARD, 0);
|
46 |
motor_r_set(FORWARD, 0);
|
47 |
return NORMAL;
|
48 |
} |
49 |
|
50 |
|
51 |
int code;
|
52 |
switch(state[0]) |
53 |
{ |
54 |
case ISTRAIGHT:
|
55 |
code = lineFollow(speed); |
56 |
if(code==INTERSECTION)
|
57 |
{ |
58 |
for(int i=0; i<4; i++) state[i]=state[i+1]; |
59 |
state[4]=0; |
60 |
if(state[0]==0)stateCounter++; |
61 |
break;
|
62 |
} |
63 |
else if(code==NOBARCODE) return NORMAL; |
64 |
return code;
|
65 |
|
66 |
|
67 |
case ILEFT:
|
68 |
code = turnLeft(); |
69 |
if(code==0) |
70 |
{ |
71 |
state[0]=0; |
72 |
stateCounter++; |
73 |
} |
74 |
break;
|
75 |
|
76 |
case IRIGHT:
|
77 |
code = turnRight(); |
78 |
if(code==0) |
79 |
{ |
80 |
state[0]=0; |
81 |
stateCounter++; |
82 |
} |
83 |
break;
|
84 |
|
85 |
|
86 |
case MERGELEFT:
|
87 |
code = mergeLeft(); |
88 |
if(code==0) |
89 |
{ |
90 |
state[0]=0; |
91 |
stateLength=0;
|
92 |
return FINISHED;
|
93 |
} |
94 |
return NORMAL;
|
95 |
|
96 |
case MERGERIGHT:
|
97 |
code = mergeRight(); |
98 |
if(code==0) |
99 |
{ |
100 |
state[0]=0; |
101 |
stateLength=0;
|
102 |
return FINISHED;
|
103 |
} |
104 |
return NORMAL;
|
105 |
|
106 |
default:
|
107 |
return LOST;
|
108 |
|
109 |
} |
110 |
|
111 |
if(stateCounter>=stateLength)
|
112 |
{ |
113 |
stateCounter=stateLength=0;
|
114 |
return FINISHED;
|
115 |
} |
116 |
return NORMAL;
|
117 |
} |
118 |
|
119 |
|
120 |
/** Starts the line-drive process if paused. */
|
121 |
void start(void){stopped=0;} |
122 |
|
123 |
/** Pauses the line-drive process. Default is started. */
|
124 |
void stop(void){stopped=1;} |
125 |
|
126 |
|
127 |
/**
|
128 |
* Defines a merge command in the direction specified. A merge is a switch
|
129 |
* of lanes.
|
130 |
* @param dir Left or right, defined by ILEFT or IRIGHT
|
131 |
*/
|
132 |
int merge(int dir) |
133 |
{ |
134 |
if(stateLength!=0)return ERROR; |
135 |
stateLength++; |
136 |
state[0]=(dir==ILEFT ? MERGELEFT : MERGERIGHT);
|
137 |
return NORMAL;
|
138 |
} |
139 |
|
140 |
/**
|
141 |
* Executes an intersection turn where the intersection type is specified by the
|
142 |
* parameters.
|
143 |
* @param type A valid defined intersection type
|
144 |
* @param dir The direction to turn at the intersection
|
145 |
*/
|
146 |
int turn(int type, int dir) |
147 |
{ |
148 |
if(stateLength!=0)return ERROR; |
149 |
if(dir==IRIGHT)
|
150 |
{ |
151 |
stateLength++; |
152 |
state[1]=IRIGHT;
|
153 |
return NORMAL;
|
154 |
} |
155 |
if(dir==IUTURN)
|
156 |
{ |
157 |
stateLength+=2;
|
158 |
state[1]=state[2]=ILEFT; |
159 |
return NORMAL;
|
160 |
} |
161 |
if(dir==ISTRAIGHT && type==SINGLE)
|
162 |
{ |
163 |
stateLength++; |
164 |
state[1]=ISTRAIGHT;
|
165 |
return NORMAL;
|
166 |
} |
167 |
if(dir==ISTRAIGHT)
|
168 |
{ |
169 |
stateLength+=2;
|
170 |
state[1]=state[2]=ISTRAIGHT; |
171 |
return NORMAL;
|
172 |
} |
173 |
// At this point, must be left turn
|
174 |
if(type==SINGLE)
|
175 |
{ |
176 |
stateLength++; |
177 |
state[1]=ILEFT;
|
178 |
return NORMAL;
|
179 |
} |
180 |
if(type==DOUBLE_C || type==DOUBLE_T)
|
181 |
{ |
182 |
stateLength+=3;
|
183 |
state[1]=state[3]=ISTRAIGHT; |
184 |
state[2]=ILEFT;
|
185 |
return NORMAL;
|
186 |
} |
187 |
if(type==ON_RAMP)
|
188 |
{ |
189 |
stateLength+=2;
|
190 |
state[1]=ILEFT;
|
191 |
state[2]=ISTRAIGHT;
|
192 |
return NORMAL;
|
193 |
} |
194 |
if(type==OFF_RAMP)
|
195 |
{ |
196 |
stateLength+=2;
|
197 |
state[1]=ISTRAIGHT;
|
198 |
state[2]=ILEFT;
|
199 |
return NORMAL;
|
200 |
} |
201 |
|
202 |
//Should never get here
|
203 |
return ERROR;
|
204 |
} |
205 |
|