Revision c76baf23 devices/DiWheelDrive/userthread.cpp
devices/DiWheelDrive/userthread.cpp | ||
---|---|---|
2 | 2 |
|
3 | 3 |
#include "global.hpp" |
4 | 4 |
|
5 |
#include "linefollow2.hpp" |
|
6 |
|
|
5 | 7 |
using namespace amiro; |
6 | 8 |
|
7 | 9 |
extern Global global; |
... | ... | |
55 | 57 |
int policyCounter = 0; // Do not change this, it points to the beginning of the policy |
56 | 58 |
|
57 | 59 |
// Different speed settings (all values in "rounds per minute") |
58 |
const int rpmForward[2] = {25,25}; |
|
59 |
const int rpmSoftLeft[2] = {15,25}; |
|
60 |
const int rpmHardLeft[2] = {10,25}; |
|
61 |
const int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]}; |
|
62 |
const int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]}; |
|
63 | 60 |
const int rpmTurnLeft[2] = {-10, 10}; |
64 | 61 |
const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]}; |
65 | 62 |
const int rpmHalt[2] = {0, 0}; |
... | ... | |
111 | 108 |
void copyRpmSpeed(const int (&source)[2], int (&target)[2]) { |
112 | 109 |
target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL]; |
113 | 110 |
target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL]; |
111 |
chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]); |
|
114 | 112 |
} |
115 | 113 |
|
116 | 114 |
// Fuzzyfication of the sensor values |
... | ... | |
198 | 196 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK && |
199 | 197 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) { |
200 | 198 |
// straight |
201 |
copyRpmSpeed(rpmForward, rpmFuzzyCtrl); |
|
199 |
copyRpmSpeed(global.rpmForward, rpmFuzzyCtrl);
|
|
202 | 200 |
// exact one front sensor detects a line |
203 | 201 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK || |
204 | 202 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) { |
205 | 203 |
// soft correction |
206 | 204 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) { |
207 | 205 |
// soft right |
208 |
copyRpmSpeed(rpmSoftRight, rpmFuzzyCtrl); |
|
206 |
copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl);
|
|
209 | 207 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) { |
210 | 208 |
// hard right |
211 |
copyRpmSpeed(rpmHardRight, rpmFuzzyCtrl); |
|
209 |
copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl);
|
|
212 | 210 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
213 | 211 |
// soft left |
214 |
copyRpmSpeed(rpmSoftLeft, rpmFuzzyCtrl); |
|
212 |
copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl);
|
|
215 | 213 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) { |
216 | 214 |
// hard left |
217 |
copyRpmSpeed(rpmHardLeft, rpmFuzzyCtrl); |
|
215 |
copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl);
|
|
218 | 216 |
} |
219 | 217 |
// both wheel sensors detect a line |
220 | 218 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK && |
... | ... | |
288 | 286 |
} |
289 | 287 |
} |
290 | 288 |
|
289 |
//void lineFollowing_new(xyz) {} |
|
290 |
|
|
291 |
void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]){ |
|
292 |
// all sensors are equal |
|
293 |
// if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] && |
|
294 |
// member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] && |
|
295 |
// member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) { |
|
296 |
// // something is wrong -> stop |
|
297 |
// copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
|
298 |
// // both front sensor detect a line |
|
299 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK && |
|
300 |
(member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY)) { |
|
301 |
// straight |
|
302 |
copyRpmSpeed(global.rpmForward, rpmFuzzyCtrl); |
|
303 |
// Deviation to right |
|
304 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK |
|
305 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){ |
|
306 |
copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl); |
|
307 |
// Deviation to left |
|
308 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK |
|
309 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){ |
|
310 |
copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl); |
|
311 |
// Hard deviatio to right |
|
312 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY |
|
313 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){ |
|
314 |
copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl); |
|
315 |
// Hard deviation to left |
|
316 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY |
|
317 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){ |
|
318 |
copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl); |
|
319 |
// Turn if white |
|
320 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE |
|
321 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE ){ |
|
322 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
|
323 |
} |
|
324 |
} |
|
325 |
|
|
291 | 326 |
// Line following by a fuzzy controler |
292 | 327 |
void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) { |
293 | 328 |
// FUZZYFICATION |
... | ... | |
312 | 347 |
global.robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT])); |
313 | 348 |
global.robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT])); |
314 | 349 |
|
315 |
// chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftFuzzyMemberValues[BLACK], leftFuzzyMemberValues[GREY], leftFuzzyMemberValues[WHITE]);
|
|
316 |
// chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
|
|
350 |
// chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftWheelFuzzyMemberValues[BLACK], leftWheelFuzzyMemberValues[GREY], leftWheelFuzzyMemberValues[WHITE]);
|
|
351 |
// chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
|
|
317 | 352 |
|
318 | 353 |
// DEFUZZYFICATION |
319 |
defuzzyfication(member, rpmFuzzyCtrl); |
|
354 |
// defuzzyfication(member, rpmFuzzyCtrl); |
|
355 |
defuzz(member, rpmFuzzyCtrl); |
|
320 | 356 |
} |
321 | 357 |
|
358 |
|
|
359 |
|
|
360 |
|
|
322 | 361 |
// Set the speed by the array |
323 | 362 |
void setRpmSpeed(const int (&rpmSpeed)[2]) { |
324 | 363 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
... | ... | |
355 | 394 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
356 | 395 |
} |
357 | 396 |
running = false; |
358 |
|
|
397 |
LineFollow lf; |
|
359 | 398 |
/* |
360 | 399 |
* LOOP |
361 | 400 |
*/ |
... | ... | |
369 | 408 |
/* |
370 | 409 |
* evaluate the accelerometer |
371 | 410 |
*/ |
372 |
if (accel_z < -900 /*-0.9g*/) { |
|
411 |
|
|
412 |
//Features over can: (Line Following) |
|
413 |
//SetKP |
|
414 |
//SetKI |
|
415 |
//SetKd ? |
|
416 |
//SetSleepTime |
|
417 |
//SeForwardSpeed? or SetMaxSpeed |
|
418 |
//DriveOnLeftLine |
|
419 |
//DriveOnRightLine |
|
420 |
|
|
421 |
//if accel_z<-900 |
|
422 |
//send can event (one time) |
|
423 |
// Line following is started if amiro roteted |
|
424 |
if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature |
|
373 | 425 |
if (running) { |
374 | 426 |
// stop the robot |
375 | 427 |
running = false; |
... | ... | |
400 | 452 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
401 | 453 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
402 | 454 |
} |
403 |
|
|
404 |
// chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n", |
|
405 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
|
406 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
|
407 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
|
408 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]); |
|
409 |
|
|
455 |
lf.followLine(vcnl4020Proximity, &global); |
|
456 |
// chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n", |
|
457 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
|
458 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
|
459 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
|
460 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]); |
|
461 |
//if (configLineFollowing==2) |
|
462 |
// lineFollownew |
|
463 |
//else |
|
410 | 464 |
lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl); |
411 | 465 |
setRpmSpeed(rpmFuzzyCtrl); |
412 | 466 |
} |
Also available in: Unified diff