root / trunk / code / projects / linefollowing / lineDrive.c @ 1931
History | View | Annotate | Download (3.83 KB)
1 | 1864 | azirbel | /**
|
---|---|---|---|
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 | 1855 | djacobs | #include "lineDrive.h" |
13 | |||
14 | 1864 | azirbel | int state[5]; //! Stores a queue of sub-commands to be executed |
15 | 1855 | djacobs | int stateCounter;
|
16 | int stateLength;
|
||
17 | 1864 | azirbel | |
18 | //! Whether lineDrive is currently paused. Set to 0 on initialization.
|
||
19 | 1855 | djacobs | int stopped=1; |
20 | |||
21 | |||
22 | 1864 | azirbel | /**
|
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 | 1855 | djacobs | 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 | 1864 | azirbel | /**
|
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 | 1855 | djacobs | int doDrive(int speed) |
42 | { |
||
43 | 1856 | djacobs | if(stopped)
|
44 | { |
||
45 | motor_l_set(FORWARD, 0);
|
||
46 | 1857 | djacobs | motor_r_set(FORWARD, 0);
|
47 | 1856 | djacobs | return NORMAL;
|
48 | } |
||
49 | 1855 | djacobs | |
50 | 1856 | djacobs | |
51 | 1855 | djacobs | 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 | 1864 | azirbel | /** Starts the line-drive process if paused. */
|
121 | 1855 | djacobs | void start(void){stopped=0;} |
122 | 1864 | azirbel | |
123 | /** Pauses the line-drive process. Default is started. */
|
||
124 | 1855 | djacobs | void stop(void){stopped=1;} |
125 | |||
126 | |||
127 | 1864 | azirbel | /**
|
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 | 1855 | djacobs | 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 | 1864 | azirbel | /**
|
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 | 1855 | djacobs | int turn(int type, int dir) |
147 | { |
||
148 | if(stateLength!=0)return ERROR; |
||
149 | 1931 | djacobs | 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 | 1855 | djacobs | |
202 | //Should never get here
|
||
203 | return ERROR;
|
||
204 | } |