Revision b4885314 devices/DiWheelDrive/userthread.cpp
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 76 | 76 |
|
| 77 | 77 |
const int blackStartFalling = 0x1000; // Where the black curve starts falling |
| 78 | 78 |
const int blackOff = 0x1800; // Where no more black is detected |
| 79 |
const int whiteStartRising = 0x4000; // Where the white curve starts rising
|
|
| 80 |
const int whiteOn = 0x8000; // Where the white curve has reached the maximum value
|
|
| 79 |
const int whiteStartRising = 0x2800; // Where the white curve starts rising
|
|
| 80 |
const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
|
|
| 81 | 81 |
const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum |
| 82 | 82 |
const int greyStartRising = blackStartFalling; // Where grey starts rising |
| 83 | 83 |
const int greyOff = whiteOn; // Where grey is completely off again |
| ... | ... | |
| 188 | 188 |
// Get a crisp output for the steering commands |
| 189 | 189 |
void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) {
|
| 190 | 190 |
|
| 191 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK && |
|
| 192 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
|
|
| 191 |
// all sensors are equal |
|
| 192 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] && |
|
| 193 |
member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] && |
|
| 194 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
|
|
| 195 |
// something is wrong -> stop |
|
| 196 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
|
| 197 |
// both front sensor detect a line |
|
| 198 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK && |
|
| 199 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
|
|
| 193 | 200 |
// straight |
| 194 | 201 |
copyRpmSpeed(rpmForward, rpmFuzzyCtrl); |
| 202 |
// exact one front sensor detects a line |
|
| 195 | 203 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK || |
| 196 | 204 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
|
| 197 | 205 |
// soft correction |
| 198 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) |
|
| 206 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
|
|
| 199 | 207 |
// soft right |
| 200 | 208 |
copyRpmSpeed(rpmSoftRight, rpmFuzzyCtrl); |
| 201 |
else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE)
|
|
| 209 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) {
|
|
| 202 | 210 |
// hard right |
| 203 | 211 |
copyRpmSpeed(rpmHardRight, rpmFuzzyCtrl); |
| 204 |
else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY)
|
|
| 212 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
|
|
| 205 | 213 |
// soft left |
| 206 | 214 |
copyRpmSpeed(rpmSoftLeft, rpmFuzzyCtrl); |
| 207 |
else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE)
|
|
| 215 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {
|
|
| 208 | 216 |
// hard left |
| 209 | 217 |
copyRpmSpeed(rpmHardLeft, rpmFuzzyCtrl); |
| 210 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY || |
|
| 218 |
} |
|
| 219 |
// both wheel sensors detect a line |
|
| 220 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK && |
|
| 221 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
|
|
| 222 |
// something is wrong -> stop |
|
| 223 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
|
| 224 |
// exactly one wheel sensor detects a line |
|
| 225 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK || |
|
| 226 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
|
|
| 227 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {
|
|
| 228 |
// turn left |
|
| 229 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
|
| 230 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
|
|
| 231 |
// turn right |
|
| 232 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
|
| 233 |
} |
|
| 234 |
// both front sensors may detect a line |
|
| 235 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY && |
|
| 211 | 236 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
|
| 212 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) |
|
| 237 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
|
|
| 238 |
// turn left |
|
| 239 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
|
| 240 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
|
|
| 213 | 241 |
// turn right |
| 214 | 242 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
| 215 |
else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) |
|
| 243 |
} |
|
| 244 |
// exactly one front sensor may detect a line |
|
| 245 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY || |
|
| 246 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
|
|
| 247 |
if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
|
|
| 216 | 248 |
// turn left |
| 217 | 249 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
| 218 |
else |
|
| 219 |
// go straight |
|
| 220 |
copyRpmSpeed(rpmForward, rpmFuzzyCtrl); |
|
| 221 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE && |
|
| 222 |
member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {
|
|
| 223 |
// go straight and check wheel sensors |
|
| 224 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] != WHITE) |
|
| 250 |
} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
|
|
| 251 |
// turn right |
|
| 252 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
|
| 253 |
} |
|
| 254 |
// both wheel sensors may detect a line |
|
| 255 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY && |
|
| 256 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
|
|
| 257 |
// something is wrong -> stop |
|
| 258 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
|
| 259 |
// exactly one wheel sensor may detect a line |
|
| 260 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY || |
|
| 261 |
member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
|
|
| 262 |
if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
|
|
| 225 | 263 |
// turn left |
| 226 | 264 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
| 227 |
else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] != WHITE)
|
|
| 265 |
} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
|
|
| 228 | 266 |
// turn right |
| 229 | 267 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
| 230 |
else |
|
| 231 |
// line is lost -> stop |
|
| 232 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
|
| 268 |
} |
|
| 269 |
// no sensor detects anything |
|
| 270 |
} else {
|
|
| 271 |
// line is lost -> stop |
|
| 272 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
|
| 233 | 273 |
} |
| 234 | 274 |
|
| 235 | 275 |
return; |
| ... | ... | |
| 371 | 411 |
setRpmSpeed(rpmFuzzyCtrl); |
| 372 | 412 |
} |
| 373 | 413 |
|
| 374 |
this->sleep(MS2ST(100));
|
|
| 414 |
this->sleep(MS2ST(10)); |
|
| 375 | 415 |
} |
| 376 | 416 |
|
| 377 | 417 |
return RDY_OK; |
Also available in: Unified diff