root / scout / libscout / src / behaviors / lineDrive.cpp @ d37cfc69
History | View | Annotate | Download (2.89 KB)
1 | 9295436d | James Carroll | /**
|
---|---|---|---|
2 | 60b98383 | Priya | * @file line_drive.cpp
|
3 | 9295436d | James Carroll | *
|
4 | * Provides functions to implement line driving behavior. This program extends
|
||
5 | * the behavior of the line-following program by following lines automatically
|
||
6 | 60b98383 | Priya | * and implementing behaviors to deal with commands passed to line_drive.
|
7 | 9295436d | James Carroll | *
|
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 | 60b98383 | Priya | #include "line_drive.h" |
17 | 9295436d | James Carroll | |
18 | 31be19a6 | Priya | line_drive::line_drive(String scoutname): Behavior(scoutname, "line_drive")
|
19 | 60b98383 | Priya | { |
20 | line_drive_init(); |
||
21 | line_follow_init(); |
||
22 | return;
|
||
23 | }; |
||
24 | 9295436d | James Carroll | |
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 | 60b98383 | Priya | void line_drive_init()
|
31 | 9295436d | James Carroll | { |
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 | aa84b67c | James Carroll | case IRIGHT:
|
79 | code = turnRight(); |
||
80 | if(code==0) |
||
81 | { |
||
82 | 9295436d | James Carroll | state[0]=0; |
83 | stateCounter++; |
||
84 | } |
||
85 | aa84b67c | James Carroll | break;
|
86 | 9295436d | James Carroll | |
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 | aa84b67c | James Carroll | if(dir==ISTRAIGHT)
|
127 | { |
||
128 | stateLength++; |
||
129 | state[1]=ISTRAIGHT;
|
||
130 | return NORMAL;
|
||
131 | } |
||
132 | 9295436d | James Carroll | if(dir==IUTURN)
|
133 | aa84b67c | James Carroll | { |
134 | stateLength+=2;
|
||
135 | state[1]=state[2]=ILEFT; |
||
136 | return NORMAL;
|
||
137 | } |
||
138 | 9295436d | James Carroll | |
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 | } |