Revision fbcb25cc devices/DiWheelDrive/userthread.cpp
devices/DiWheelDrive/userthread.cpp | ||
---|---|---|
69 | 69 |
} |
70 | 70 |
} |
71 | 71 |
|
72 |
bool UserThread::checkFrontalObject(){ |
|
73 |
uint32_t thresh = 1000; |
|
74 |
uint32_t prox; |
|
75 |
for(int i=0; i<8; i++){ |
|
76 |
prox = global.robot.getProximityRingValue(i); |
|
77 |
if((i == 3) || (i == 4)){ |
|
78 |
if(prox < thresh){ |
|
79 |
return false; |
|
80 |
} |
|
81 |
}else{ |
|
82 |
if(prox > thresh){ |
|
83 |
return false; |
|
84 |
} |
|
85 |
} |
|
86 |
} |
|
87 |
return true; |
|
88 |
} |
|
89 |
|
|
72 | 90 |
bool UserThread::checkPinVoltage(){ |
73 | 91 |
return global.ltc4412.isPluggedIn(); |
74 | 92 |
} |
... | ... | |
128 | 146 |
* SETUP |
129 | 147 |
*/ |
130 | 148 |
// User thread state: |
131 |
states utState = states::IDLE; |
|
132 |
states newState; |
|
133 | 149 |
|
134 |
int whiteBuf = 0; |
|
135 |
int proxyBuf = 0; |
|
150 |
|
|
151 |
// int whiteBuf = 0; |
|
152 |
// int proxyBuf = 0; |
|
136 | 153 |
// int reverseBuf = 0; |
137 | 154 |
int correctionStep = 0; |
138 | 155 |
// int dist_count = 0; |
... | ... | |
161 | 178 |
if (accel_z < -900 /*-0.9g*/) { |
162 | 179 |
// Start line following when AMiRo is rotated |
163 | 180 |
if(utState == states::IDLE){ |
164 |
utState = states::FOLLOW_LINE;
|
|
181 |
newState = states::FOLLOW_LINE;
|
|
165 | 182 |
}else{ |
166 |
utState = states::IDLE;
|
|
183 |
newState = states::IDLE;
|
|
167 | 184 |
} |
168 | 185 |
lightAllLeds(Color::GREEN); |
169 | 186 |
this->sleep(1000); |
... | ... | |
175 | 192 |
// running = true; |
176 | 193 |
switch(global.lfStrategy){ |
177 | 194 |
case msg_content::START: |
178 |
utState = states::FOLLOW_LINE;
|
|
195 |
newState = states::FOLLOW_LINE;
|
|
179 | 196 |
break; |
180 | 197 |
case msg_content::STOP: |
181 |
utState = states::IDLE;
|
|
198 |
newState = states::IDLE;
|
|
182 | 199 |
break; |
183 | 200 |
case msg_content::EDGE_RIGHT: |
184 |
// utState = states::FOLLOW_LINE;
|
|
201 |
// newState = states::FOLLOW_LINE;
|
|
185 | 202 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
186 | 203 |
break; |
187 | 204 |
case msg_content::EDGE_LEFT: |
188 |
// utState = states::FOLLOW_LINE;
|
|
205 |
// newState = states::FOLLOW_LINE;
|
|
189 | 206 |
lStrategy = LineFollowStrategy::EDGE_LEFT; |
190 | 207 |
break; |
191 | 208 |
case msg_content::FUZZY: |
192 |
// utState = states::FOLLOW_LINE;
|
|
209 |
// newState = states::FOLLOW_LINE;
|
|
193 | 210 |
lStrategy = LineFollowStrategy::FUZZY; |
194 | 211 |
break; |
195 | 212 |
case msg_content::DOCK: |
196 |
utState = states::DETECT_STATION;
|
|
213 |
newState = states::DETECT_STATION;
|
|
197 | 214 |
break; |
198 | 215 |
case msg_content::UNDOCK: |
199 |
utState = states::RELEASE;
|
|
216 |
newState = states::RELEASE;
|
|
200 | 217 |
break; |
201 | 218 |
case msg_content::CHARGE: |
202 |
utState = states::CHARGING;
|
|
219 |
newState = states::CHARGING;
|
|
203 | 220 |
break; |
204 | 221 |
default: |
205 |
utState = states::IDLE;
|
|
222 |
newState = states::IDLE;
|
|
206 | 223 |
break; |
207 | 224 |
} |
208 | 225 |
} |
209 |
newState = utState; |
|
226 |
// newState = utState;
|
|
210 | 227 |
|
211 | 228 |
// Get sensor data |
212 |
uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
213 |
uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset(); |
|
229 |
// uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
|
|
230 |
// uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
|
|
214 | 231 |
for(int i=0; i<8;i++){ |
215 | 232 |
rProx[i] = global.robot.getProximityRingValue(i); |
216 | 233 |
} |
... | ... | |
223 | 240 |
if(/* checkPinVoltage() && */ checkPinEnabled()){ |
224 | 241 |
global.robot.requestCharging(0); |
225 | 242 |
} |
226 |
whiteBuf = 0;
|
|
227 |
proxyBuf = 0;
|
|
243 |
utCount.whiteCount = 0;
|
|
244 |
utCount.proxyCount = 0;
|
|
228 | 245 |
utCount.errorCount = 0; |
246 |
utCount.idleCount++; |
|
247 |
if (utCount.idleCount > 10){ |
|
248 |
utCount.idleCount = 0; |
|
249 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
|
250 |
global.robot.transmitState(utState); |
|
251 |
} |
|
229 | 252 |
break; |
230 | 253 |
// --------------------------------------- |
231 | 254 |
case states::FOLLOW_LINE: |
... | ... | |
238 | 261 |
lf.setStrategy(lStrategy); |
239 | 262 |
} |
240 | 263 |
|
241 |
//TODO: Check if white is detected and stop threshold is reached |
|
242 | 264 |
if(lf.followLine(rpmSpeed)){ |
243 |
whiteBuf++;
|
|
244 |
if(whiteBuf >= WHITE_COUNT_THRESH){
|
|
265 |
utCount.whiteCount++;
|
|
266 |
if(utCount.whiteCount >= WHITE_COUNT_THRESH){
|
|
245 | 267 |
rpmSpeed[0] = stop[0]; |
246 | 268 |
rpmSpeed[1] = stop[1]; |
247 | 269 |
newState = states::IDLE; |
248 | 270 |
} |
249 | 271 |
}else{ |
250 |
whiteBuf = 0;
|
|
272 |
utCount.whiteCount = 0;
|
|
251 | 273 |
// setRpmSpeed(rpmSpeed); |
252 | 274 |
} |
253 | 275 |
|
254 | 276 |
if(getProxyRingSum() > PROXY_RING_THRESH){ |
255 |
proxyBuf++; |
|
256 |
if(proxyBuf > WHITE_COUNT_THRESH){ |
|
257 |
newState = states::IDLE; |
|
277 |
utCount.proxyCount++; |
|
278 |
if(utCount.proxyCount > WHITE_COUNT_THRESH){ |
|
258 | 279 |
rpmSpeed[0] = stop[0]; |
259 | 280 |
rpmSpeed[1] = stop[1]; |
281 |
// newState = states::IDLE; |
|
282 |
if (continue_on_obstacle){ |
|
283 |
newState = states::TURN; |
|
284 |
utCount.proxyCount = 0; |
|
285 |
}else{ |
|
286 |
newState = states::IDLE; |
|
287 |
} |
|
260 | 288 |
} |
261 | 289 |
}else{ |
262 |
proxyBuf = 0;
|
|
290 |
utCount.proxyCount = 0;
|
|
263 | 291 |
} |
264 | 292 |
|
265 | 293 |
if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){ |
... | ... | |
267 | 295 |
rpmSpeed[1] = stop[1]; |
268 | 296 |
} |
269 | 297 |
|
270 |
// if (needDistance){ |
|
271 |
// whiteBuf = 0; |
|
272 |
// proxyBuf = 0; |
|
273 |
// dist_count++; |
|
274 |
// if(dist_count > DIST_THRESH){ |
|
275 |
// dist_count = 0; |
|
276 |
// needDistance = false; |
|
277 |
// } |
|
278 |
// } |
|
279 |
// whiteBuf = 0; |
|
280 |
// proxyBuf = 0; |
|
281 |
// lf.followLine(rpmSpeed); |
|
282 | 298 |
if (lf.getStrategy() == LineFollowStrategy::FUZZY){ |
283 | 299 |
setRpmSpeedFuzzy(rpmSpeed); |
284 | 300 |
}else{ |
... | ... | |
288 | 304 |
|
289 | 305 |
break; |
290 | 306 |
// --------------------------------------- |
307 |
case states::TURN: |
|
308 |
checkForMotion(); |
|
309 |
// Check if only front sensors are active |
|
310 |
if(checkFrontalObject()){ |
|
311 |
global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION); |
|
312 |
// BaseThread::sleep(8000); |
|
313 |
checkForMotion(); |
|
314 |
newState = states::FOLLOW_LINE; |
|
315 |
lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
|
316 |
}else{ |
|
317 |
newState = states::IDLE; |
|
318 |
} |
|
319 |
break; |
|
320 |
// --------------------------------------- |
|
291 | 321 |
case states::DETECT_STATION: |
292 | 322 |
if (global.forwardSpeed != DETECTION_SPEED){ |
293 | 323 |
global.forwardSpeed = DETECTION_SPEED; |
... | ... | |
323 | 353 |
lf.followLine(rpmSpeed); |
324 | 354 |
setRpmSpeed(rpmSpeed); |
325 | 355 |
|
326 |
correctionStep++;
|
|
327 |
if (correctionStep >= MAX_CORRECTION_STEPS){
|
|
328 |
correctionStep = 0;
|
|
356 |
utCount.stepCount++;
|
|
357 |
if (utCount.stepCount >= MAX_CORRECTION_STEPS){
|
|
358 |
utCount.stepCount = 0;
|
|
329 | 359 |
newState = states::REVERSE; |
330 | 360 |
setRpmSpeed(stop); |
331 | 361 |
checkForMotion(); |
... | ... | |
352 | 382 |
if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){ |
353 | 383 |
// setRpmSpeed(stop); |
354 | 384 |
// checkForMotion(); |
355 |
utCount.reverseCount = 0;
|
|
385 |
utCount.stepCount = 0;
|
|
356 | 386 |
newState = states::PUSH_BACK; |
357 | 387 |
} |
358 | 388 |
break; |
... | ... | |
364 | 394 |
lf.followLine(rpmSpeed); |
365 | 395 |
setRpmSpeed(rpmSpeed); |
366 | 396 |
|
367 |
utCount.reverseCount++;
|
|
368 |
if (utCount.reverseCount > PUSH_BACK_COUNT){
|
|
397 |
utCount.stepCount++;
|
|
398 |
if (utCount.stepCount > PUSH_BACK_COUNT){
|
|
369 | 399 |
newState = states::CHECK_POSITIONING; |
370 | 400 |
} |
371 | 401 |
break; |
... | ... | |
435 | 465 |
// --------------------------------------- |
436 | 466 |
case states::ERROR: |
437 | 467 |
utCount.errorCount = 0; |
438 |
lStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
439 |
newState = states::FOLLOW_LINE;
|
|
468 |
// lStrategy = LineFollowStrategy::EDGE_RIGHT;
|
|
469 |
newState = states::RELEASE;
|
|
440 | 470 |
break; |
441 | 471 |
// --------------------------------------- |
442 | 472 |
case states::CHARGING: |
... | ... | |
485 | 515 |
break; |
486 | 516 |
} |
487 | 517 |
if (utState != newState){ |
518 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
|
488 | 519 |
global.robot.transmitState(newState); |
489 | 520 |
} |
490 | 521 |
utState = newState; |
Also available in: Unified diff