Revision 1855
lineFollow now supports merge. line drive is UNTESTED and has NO COMMENTS, but will provide higher level driving control
trunk/code/projects/linefollowing/lineDrive.c | ||
---|---|---|
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 |
|
trunk/code/projects/linefollowing/lineDrive.h | ||
---|---|---|
1 |
#ifndef _LINE_DRIVE_ |
|
2 |
#define _LINE_DRIVE_ |
|
3 |
|
|
4 |
#include "lineFollow.h" |
|
5 |
|
|
6 |
#define DOUBLE 0 |
|
7 |
#define SINGLE 1 |
|
8 |
#define ON_RAMP 2 |
|
9 |
#define OFF_RAMP 3 |
|
10 |
|
|
11 |
#define ISTRAIGHT 0 |
|
12 |
#define ILEFT 1 |
|
13 |
#define IRIGHT 2 |
|
14 |
#define IUTURN 3 |
|
15 |
|
|
16 |
#define MERGELEFT 4 |
|
17 |
#define MERGERIGHT 5 |
|
18 |
|
|
19 |
#define NORMAL -1 |
|
20 |
#define FINISHED -2 |
|
21 |
#define LOST -3 |
|
22 |
#define ERROR -4 |
|
23 |
|
|
24 |
|
|
25 |
|
|
26 |
void lineDrive_init(void); |
|
27 |
|
|
28 |
|
|
29 |
|
|
30 |
int doDrive(int speed); |
|
31 |
|
|
32 |
void start(void); |
|
33 |
void stop(void); |
|
34 |
|
|
35 |
|
|
36 |
int merge(int dir); |
|
37 |
|
|
38 |
int turn(int type, int dir); |
|
39 |
|
|
40 |
#endif |
Also available in: Unified diff