Revision 1931 trunk/code/projects/linefollowing/lineDrive.c

View differences:

lineDrive.c
146 146
int turn(int type, int dir)
147 147
{
148 148
	if(stateLength!=0)return ERROR;
149
	if(dir==IRIGHT){stateLength++; state[1]=IRIGHT; return NORMAL;}
150
	if(dir==IUTURN){stateLength+=2;  state[1]=state[2]=ILEFT; return NORMAL;}
151
	if(dir==ISTRAIGHT && type==SINGLE){stateLength++; state[1]=ISTRAIGHT; return NORMAL;}
152
	if(dir==ISTRAIGHT){stateLength+=2; state[1]=state[2]=ISTRAIGHT; return NORMAL;}
153
	//must be left turn
154
	if(type==SINGLE){stateLength++; state[1]=ILEFT; return NORMAL;}
155
	if(type==DOUBLE){stateLength+=3;state[1]=state[3]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;}
156
	if(type==ON_RAMP){stateLength+=2; state[1]=ILEFT; state[2]=ISTRAIGHT; return NORMAL;}
157
	if(type==OFF_RAMP){stateLength+=2; state[1]=ISTRAIGHT; state[2]=ILEFT; return NORMAL;}
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
    }
158 201

  
159 202
	//Should never get here
160 203
	return ERROR;
161 204
}
205

  

Also available in: Unified diff