root / scout / libscout / src / behaviors / lineDrive.cpp @ 31be19a6
History | View | Annotate | Download (2.89 KB)
1 |
/**
|
---|---|
2 |
* @file line_drive.cpp
|
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 line_drive.
|
7 |
*
|
8 |
* Specific implementation for the colony scout robots in a warehouse behaviour
|
9 |
* setting.
|
10 |
*
|
11 |
* @author James Carroll(キヤロル)
|
12 |
* Cannabalized from Dan Jacobs code.
|
13 |
* @date 2-6-2012
|
14 |
*/
|
15 |
|
16 |
#include "line_drive.h" |
17 |
|
18 |
line_drive::line_drive(String scoutname): Behavior(scoutname, "line_drive")
|
19 |
{ |
20 |
line_drive_init(); |
21 |
line_follow_init(); |
22 |
return;
|
23 |
}; |
24 |
|
25 |
/**
|
26 |
* Starts the line following procedure. Must be called before other
|
27 |
* line-following functions will work. This function essentially resets the
|
28 |
* state of line-following.
|
29 |
*/
|
30 |
void line_drive_init()
|
31 |
{ |
32 |
lineFollow_init(); |
33 |
for(int i=0; i<5; i++)state[i]=0; |
34 |
stateCounter=0;
|
35 |
stateLength=0;
|
36 |
stopped=0;
|
37 |
} |
38 |
|
39 |
|
40 |
/**
|
41 |
* Follows a line and executes whatever command is next on the queue.
|
42 |
* @param speed The speed with which to drive along the line.
|
43 |
*/
|
44 |
int doDrive(int speed) |
45 |
{ |
46 |
if(stopped)
|
47 |
{ |
48 |
motors->set_sides(0,0,ABSOLUTE); |
49 |
return NORMAL;
|
50 |
} |
51 |
|
52 |
|
53 |
int code;
|
54 |
switch(state[0]) |
55 |
{ |
56 |
case ISTRAIGHT:
|
57 |
code = lineFollow(speed); |
58 |
if(code==INTERSECTION)
|
59 |
{ |
60 |
for(int i=0; i<4; i++) state[i]=state[i+1]; |
61 |
state[4]=0; |
62 |
if(state[0]==0)stateCounter++; |
63 |
break;
|
64 |
} |
65 |
else if(code==NOBARCODE) return NORMAL; |
66 |
return code;
|
67 |
|
68 |
|
69 |
case ILEFT:
|
70 |
code = turnLeft(); |
71 |
if(code==0) |
72 |
{ |
73 |
state[0]=0; |
74 |
stateCounter++; |
75 |
} |
76 |
break;
|
77 |
|
78 |
case IRIGHT:
|
79 |
code = turnRight(); |
80 |
if(code==0) |
81 |
{ |
82 |
state[0]=0; |
83 |
stateCounter++; |
84 |
} |
85 |
break;
|
86 |
|
87 |
|
88 |
default:
|
89 |
return LOST;
|
90 |
|
91 |
} |
92 |
|
93 |
if(stateCounter>=stateLength)
|
94 |
{ |
95 |
stateCounter=stateLength=0;
|
96 |
return FINISHED;
|
97 |
} |
98 |
return NORMAL;
|
99 |
} |
100 |
|
101 |
|
102 |
/** Starts the line-drive process if paused. */
|
103 |
void start(void){ |
104 |
stopped=0;
|
105 |
} |
106 |
|
107 |
/** Pauses the line-drive process. Default is started. */
|
108 |
void stop(void){ |
109 |
stopped=1;
|
110 |
} |
111 |
|
112 |
|
113 |
|
114 |
/**
|
115 |
* Executes an intersection turn where the intersection type is specified by the
|
116 |
* parameters.
|
117 |
* @param type A valid defined intersection type
|
118 |
* @param dir The direction to turn at the intersection
|
119 |
*/
|
120 |
int turn(int type, int dir) |
121 |
{ |
122 |
if(stateLength!=0) |
123 |
return ERROR;
|
124 |
|
125 |
//turns that don't depend on what kind of road your on
|
126 |
if(dir==ISTRAIGHT)
|
127 |
{ |
128 |
stateLength++; |
129 |
state[1]=ISTRAIGHT;
|
130 |
return NORMAL;
|
131 |
} |
132 |
if(dir==IUTURN)
|
133 |
{ |
134 |
stateLength+=2;
|
135 |
state[1]=state[2]=ILEFT; |
136 |
return NORMAL;
|
137 |
} |
138 |
|
139 |
if(type==ON_ROAD)
|
140 |
{ |
141 |
if(dir==IRIGHT){
|
142 |
stateLength++; |
143 |
state[1] = IRIGHT;
|
144 |
return NORMAL;
|
145 |
} |
146 |
if(dir==ILEFT){
|
147 |
stateLength += 2;
|
148 |
state[1] = ILEFT;
|
149 |
state[2] = ISTRAIGHT;
|
150 |
return NORMAL
|
151 |
} |
152 |
} |
153 |
if(type==OFF_ROAD)
|
154 |
{ |
155 |
if(dir==IRIGHT){
|
156 |
stateLength++; |
157 |
state[1]=IRIGHT;
|
158 |
return NORMAL;
|
159 |
} |
160 |
if(dir==ILEFT){
|
161 |
stateLength+=2;
|
162 |
state[1]=ISTRAIGHT;
|
163 |
state[2]=ILEFT;
|
164 |
} |
165 |
} |
166 |
|
167 |
//Should never get here
|
168 |
return ERROR;
|
169 |
} |
170 |
|