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