amiro-os / devices / DiWheelDrive / linefollow.cpp @ 726fdc72
History | View | Annotate | Download (15.1 KB)
1 |
#include "global.hpp" |
---|---|
2 |
#include "linefollow2.hpp" |
3 |
#include <cmath> |
4 |
|
5 |
|
6 |
|
7 |
LineFollow::LineFollow(Global *global){ |
8 |
this->global = global;
|
9 |
} |
10 |
LineFollow::LineFollow(Global *global, LineFollowStrategy strategy){ |
11 |
this->global = global;
|
12 |
this-> strategy = strategy;
|
13 |
} |
14 |
|
15 |
|
16 |
int LineFollow::transitionError(int FL, int FR, int targetL, int targetR){ |
17 |
// global->robot.setLightColor(0, Color::RED);
|
18 |
// global->robot.setLightColor(7, Color::RED);
|
19 |
int error = 0; |
20 |
|
21 |
switch (this->strategy) |
22 |
{ |
23 |
case LineFollowStrategy::TRANSITION_R_L:
|
24 |
error = -(FL -targetL + FR - targetR + this->trans);
|
25 |
break;
|
26 |
case LineFollowStrategy::TRANSITION_L_R:
|
27 |
error = (FL -targetL + FR - targetR + this->trans);
|
28 |
break;
|
29 |
default:
|
30 |
break;
|
31 |
} |
32 |
this->trans += 400; |
33 |
if(FL+FR <= RAND_TRESH){
|
34 |
// global->robot.setLightColor(0, Color::GREEN);
|
35 |
// global->robot.setLightColor(7, Color::GREEN);
|
36 |
switch (this->strategy) |
37 |
{ |
38 |
case LineFollowStrategy::TRANSITION_R_L:
|
39 |
this->strategy = LineFollowStrategy::EDGE_LEFT;
|
40 |
break;
|
41 |
case LineFollowStrategy::TRANSITION_L_R:
|
42 |
this->strategy = LineFollowStrategy::EDGE_RIGHT;
|
43 |
break;
|
44 |
default:
|
45 |
break;
|
46 |
} |
47 |
this->trans = 0; |
48 |
} |
49 |
return error;
|
50 |
} |
51 |
|
52 |
/**
|
53 |
* Calculate the error from front proxi sensors and fixed threshold values for those sensors.
|
54 |
*/
|
55 |
int LineFollow::getError(){
|
56 |
// global->robot.setLightColor(3, Color::YELLOW);
|
57 |
// Get actual sensor data of both front sensors
|
58 |
int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
59 |
int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
60 |
int targetL = global->threshProxyL;
|
61 |
int targetR = global->threshProxyR;
|
62 |
int error = 0; |
63 |
switch (this->strategy) |
64 |
{ |
65 |
case LineFollowStrategy::EDGE_RIGHT:
|
66 |
error = -(FL -targetL + FR - targetR); |
67 |
break;
|
68 |
case LineFollowStrategy::EDGE_LEFT:
|
69 |
error = (FL -targetL + FR - targetR); |
70 |
break;
|
71 |
case LineFollowStrategy::MIDDLE:
|
72 |
// Assume that the smallest value means driving in the middle
|
73 |
targetL = targetR = !(targetL<targetR)?targetR:targetL; |
74 |
error = (FL -targetL + FR - targetR); |
75 |
break;
|
76 |
case LineFollowStrategy::TRANSITION_L_R: case LineFollowStrategy::TRANSITION_R_L: |
77 |
error = transitionError(FL, FR, targetL, targetR); |
78 |
break;
|
79 |
default:
|
80 |
break;
|
81 |
} |
82 |
// Debugging stuff ------
|
83 |
// if (global->enableRecord){
|
84 |
// global->senseRec[global->sensSamples].error = error;
|
85 |
// global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
86 |
// global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
87 |
// global->sensSamples++;
|
88 |
// }
|
89 |
// ----------------------
|
90 |
// Register white values
|
91 |
if (FL+FR > global->threshWhite){
|
92 |
whiteFlag = 1;
|
93 |
}else{
|
94 |
whiteFlag = 0;
|
95 |
} |
96 |
return error;
|
97 |
} |
98 |
|
99 |
|
100 |
|
101 |
|
102 |
/**
|
103 |
* Depending on the strategy different behaviours will be triggered.
|
104 |
* FUZZY - standard tracking of black area
|
105 |
* REVERSE - drive back
|
106 |
* @param: rpmSpeed motor speed
|
107 |
*/
|
108 |
int LineFollow::followLine(int (&rpmSpeed)[2]){ |
109 |
|
110 |
int correctionSpeed = 0; |
111 |
switch (this->strategy) |
112 |
{ |
113 |
case LineFollowStrategy::FUZZY:
|
114 |
for (int i = 0; i < 4; i++) { |
115 |
vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight(); |
116 |
vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset(); |
117 |
} |
118 |
lineFollowing(vcnl4020Proximity, rpmSpeed); |
119 |
break;
|
120 |
|
121 |
case LineFollowStrategy::REVERSE:
|
122 |
correctionSpeed = -getPidCorrectionSpeed(); |
123 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -global->forwardSpeed; |
124 |
|
125 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -global->forwardSpeed; |
126 |
|
127 |
break;
|
128 |
|
129 |
default:
|
130 |
correctionSpeed = getPidCorrectionSpeed(); |
131 |
// chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed, global->threshWhite);
|
132 |
|
133 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = global->forwardSpeed + correctionSpeed; |
134 |
|
135 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed; |
136 |
break;
|
137 |
} |
138 |
return whiteFlag;
|
139 |
} |
140 |
|
141 |
|
142 |
/**
|
143 |
* Pid controller which returns a corrections speed.
|
144 |
*/
|
145 |
int LineFollow::getPidCorrectionSpeed(){
|
146 |
int error = getError();
|
147 |
int sloap = oldError - error ;
|
148 |
// int correctionSpeed = (int) (global->K_p*error + Ki*accumHist - global->K_d*sloap);
|
149 |
int correctionSpeed = (int) (K_p*error + K_i*accumHist - K_d*sloap); |
150 |
oldError = error; |
151 |
// accumHist += (int) (0.01 * error);
|
152 |
if (abs(error) > global->maxDist.error){
|
153 |
global->maxDist.error = error; |
154 |
} |
155 |
return correctionSpeed;
|
156 |
} |
157 |
|
158 |
|
159 |
void LineFollow::setStrategy(LineFollowStrategy strategy){
|
160 |
|
161 |
if(this->strategy == LineFollowStrategy::TRANSITION_R_L || this->strategy == LineFollowStrategy::TRANSITION_L_R){ |
162 |
return;
|
163 |
} |
164 |
|
165 |
switch(strategy){
|
166 |
case LineFollowStrategy::EDGE_LEFT:
|
167 |
if((this->strategy == LineFollowStrategy::EDGE_RIGHT) || (this->strategy == LineFollowStrategy::TRANSITION_R_L)){ |
168 |
this->strategy = LineFollowStrategy::TRANSITION_R_L;
|
169 |
}else{
|
170 |
// In case of fuzzy or reverse
|
171 |
this->strategy = strategy;
|
172 |
} |
173 |
break;
|
174 |
case LineFollowStrategy::EDGE_RIGHT:
|
175 |
if((this->strategy == LineFollowStrategy::EDGE_LEFT) || (this->strategy == LineFollowStrategy::TRANSITION_L_R)){ |
176 |
this->strategy = LineFollowStrategy::TRANSITION_L_R;
|
177 |
}else{
|
178 |
// In case of fuzzy or reverse
|
179 |
this->strategy = strategy;
|
180 |
} |
181 |
break;
|
182 |
default:
|
183 |
// From Fuzzy or Reverse state should work to transition automatically
|
184 |
this->strategy = strategy;
|
185 |
break;
|
186 |
} |
187 |
// this->strategy = strategy;
|
188 |
} |
189 |
|
190 |
void LineFollow::promptStrategyChange(LineFollowStrategy strategy){
|
191 |
this->strategy = strategy;
|
192 |
} |
193 |
|
194 |
LineFollowStrategy LineFollow::getStrategy(){ |
195 |
return this->strategy; |
196 |
} |
197 |
void LineFollow::setGains(float Kp, float Ki, float Kd){ |
198 |
this->K_p = Kp;
|
199 |
this->K_i = Ki;
|
200 |
this->K_d = Kd;
|
201 |
} |
202 |
|
203 |
|
204 |
|
205 |
|
206 |
|
207 |
// Legacy code, fuzzy following-----------------------------------------
|
208 |
// Line following by a fuzzy controler
|
209 |
void LineFollow::lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) { |
210 |
// FUZZYFICATION
|
211 |
// First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
|
212 |
float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3]; |
213 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues); |
214 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues); |
215 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues); |
216 |
fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues); |
217 |
|
218 |
// INFERENCE RULE DEFINITION
|
219 |
// Get the member for each sensor
|
220 |
colorMember member[4];
|
221 |
member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues); |
222 |
member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues); |
223 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues); |
224 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues); |
225 |
|
226 |
// visualize sensors via LEDs
|
227 |
global->robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT])); |
228 |
global->robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT])); |
229 |
global->robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT])); |
230 |
global->robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT])); |
231 |
|
232 |
// chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftWheelFuzzyMemberValues[BLACK], leftWheelFuzzyMemberValues[GREY], leftWheelFuzzyMemberValues[WHITE]);
|
233 |
// chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
|
234 |
|
235 |
// DEFUZZYFICATION
|
236 |
defuzzyfication(member, rpmFuzzyCtrl); |
237 |
// defuzz(member, rpmFuzzyCtrl);
|
238 |
} |
239 |
|
240 |
|
241 |
Color LineFollow::memberToLed(colorMember member) { |
242 |
switch (member) {
|
243 |
case BLACK:
|
244 |
return Color(Color::GREEN);
|
245 |
case GREY:
|
246 |
return Color(Color::YELLOW);
|
247 |
case WHITE:
|
248 |
return Color(Color::RED);
|
249 |
default:
|
250 |
return Color(Color::WHITE);
|
251 |
} |
252 |
} |
253 |
|
254 |
void LineFollow::defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) { |
255 |
whiteFlag = 0;
|
256 |
// all sensors are equal
|
257 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
|
258 |
member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] && |
259 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) { |
260 |
// something is wrong -> stop
|
261 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
262 |
// both front sensor detect a line
|
263 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK && |
264 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) { |
265 |
// straight
|
266 |
copyRpmSpeed(global->rpmForward, rpmFuzzyCtrl); |
267 |
// exact one front sensor detects a line
|
268 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK || |
269 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) { |
270 |
// soft correction
|
271 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
|
272 |
// soft right
|
273 |
copyRpmSpeed(global->rpmSoftRight, rpmFuzzyCtrl); |
274 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) { |
275 |
// hard right
|
276 |
copyRpmSpeed(global->rpmHardRight, rpmFuzzyCtrl); |
277 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
278 |
// soft left
|
279 |
copyRpmSpeed(global->rpmSoftLeft, rpmFuzzyCtrl); |
280 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) { |
281 |
// hard left
|
282 |
copyRpmSpeed(global->rpmHardLeft, rpmFuzzyCtrl); |
283 |
} |
284 |
// both wheel sensors detect a line
|
285 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK && |
286 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) { |
287 |
// something is wrong -> stop
|
288 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
289 |
// exactly one wheel sensor detects a line
|
290 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK || |
291 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) { |
292 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {
|
293 |
// turn left
|
294 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
295 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) { |
296 |
// turn right
|
297 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
298 |
} |
299 |
// both front sensors may detect a line
|
300 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY && |
301 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
302 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
|
303 |
// turn left
|
304 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
305 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) { |
306 |
// turn right
|
307 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
308 |
} |
309 |
// exactly one front sensor may detect a line
|
310 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY || |
311 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
312 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
|
313 |
// turn left
|
314 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
315 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
316 |
// turn right
|
317 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
318 |
} |
319 |
// both wheel sensors may detect a line
|
320 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY && |
321 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) { |
322 |
// something is wrong -> stop
|
323 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
324 |
// exactly one wheel sensor may detect a line
|
325 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY || |
326 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) { |
327 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
|
328 |
// turn left
|
329 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
330 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) { |
331 |
// turn right
|
332 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
333 |
} |
334 |
// no sensor detects anything
|
335 |
} else {
|
336 |
// line is lost -> stop
|
337 |
whiteFlag = 1;
|
338 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
339 |
} |
340 |
chprintf((BaseSequentialStream*) &SD1, "Fuzzy Speed: Left: %d, Right: %d\n", rpmFuzzyCtrl[0], rpmFuzzyCtrl[1]); |
341 |
return;
|
342 |
} |
343 |
|
344 |
colorMember LineFollow::getMember(float (&fuzzyValue)[3]) { |
345 |
colorMember member; |
346 |
|
347 |
if (fuzzyValue[BLACK] > fuzzyValue[GREY])
|
348 |
if (fuzzyValue[BLACK] > fuzzyValue[WHITE])
|
349 |
member = BLACK; |
350 |
else
|
351 |
member = WHITE; |
352 |
else
|
353 |
if (fuzzyValue[GREY] > fuzzyValue[WHITE])
|
354 |
member = GREY; |
355 |
else
|
356 |
member = WHITE; |
357 |
|
358 |
return member;
|
359 |
} |
360 |
|
361 |
// Fuzzyfication of the sensor values
|
362 |
void LineFollow::fuzzyfication(int sensorValue, float (&fuzziedValue)[3]) { |
363 |
if (sensorValue < blackStartFalling ) {
|
364 |
// Only black value
|
365 |
fuzziedValue[BLACK] = 1.0f; |
366 |
fuzziedValue[GREY] = 0.0f; |
367 |
fuzziedValue[WHITE] = 0.0f; |
368 |
} else if (sensorValue > whiteOn ) { |
369 |
// Only white value
|
370 |
fuzziedValue[BLACK] = 0.0f; |
371 |
fuzziedValue[GREY] = 0.0f; |
372 |
fuzziedValue[WHITE] = 1.0f; |
373 |
} else if ( sensorValue < greyMax) { |
374 |
// Some greyisch value between black and grey
|
375 |
|
376 |
// Black is going down
|
377 |
if ( sensorValue > blackOff) {
|
378 |
fuzziedValue[BLACK] = 0.0f; |
379 |
} else {
|
380 |
fuzziedValue[BLACK] = static_cast<float>(sensorValue-blackOff) / (blackStartFalling-blackOff); |
381 |
} |
382 |
|
383 |
// Grey is going up
|
384 |
if ( sensorValue < greyStartRising) {
|
385 |
fuzziedValue[GREY] = 0.0f; |
386 |
} else {
|
387 |
fuzziedValue[GREY] = static_cast<float>(sensorValue-greyStartRising) / (greyMax-greyStartRising); |
388 |
} |
389 |
|
390 |
// White is absent
|
391 |
fuzziedValue[WHITE] = 0.0f; |
392 |
|
393 |
} else if ( sensorValue >= greyMax) { |
394 |
// Some greyisch value between grey white
|
395 |
|
396 |
// Black is absent
|
397 |
fuzziedValue[BLACK] = 0.0f; |
398 |
|
399 |
// Grey is going down
|
400 |
if ( sensorValue < greyOff) {
|
401 |
fuzziedValue[GREY] = static_cast<float>(sensorValue-greyOff) / (greyMax-greyOff); |
402 |
} else {
|
403 |
fuzziedValue[GREY] = 0.0f; |
404 |
} |
405 |
|
406 |
// White is going up
|
407 |
if ( sensorValue < whiteStartRising) {
|
408 |
fuzziedValue[WHITE] = 0.0f; |
409 |
} else {
|
410 |
fuzziedValue[WHITE] = static_cast<float>(sensorValue-whiteStartRising) / (whiteOn-whiteStartRising); |
411 |
} |
412 |
} |
413 |
} |
414 |
|
415 |
void LineFollow::copyRpmSpeed(const int (&source)[2], int (&target)[2]) { |
416 |
target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL]; |
417 |
target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL]; |
418 |
// chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
|
419 |
} |