amiro-os / devices / DiWheelDrive / linefollow.cpp @ 0f37fb41
History | View | Annotate | Download (15.173 KB)
| 1 | 726fdc72 | galberding | #include "global.hpp" |
|---|---|---|---|
| 2 | d314ad6f | galberding | #include "linefollow.hpp" |
| 3 | 726fdc72 | galberding | #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 | c9fa414d | galberding | rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -1000000*global->forwardSpeed;
|
| 124 | 726fdc72 | galberding | |
| 125 | c9fa414d | galberding | rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -1000000*global->forwardSpeed;
|
| 126 | 726fdc72 | galberding | |
| 127 | break;
|
||
| 128 | |||
| 129 | default:
|
||
| 130 | correctionSpeed = getPidCorrectionSpeed(); |
||
| 131 | // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed, global->threshWhite);
|
||
| 132 | |||
| 133 | c9fa414d | galberding | rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = 1000000*global->forwardSpeed + correctionSpeed;
|
| 134 | 726fdc72 | galberding | |
| 135 | c9fa414d | galberding | rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = 1000000*global->forwardSpeed - correctionSpeed;
|
| 136 | 726fdc72 | galberding | break;
|
| 137 | } |
||
| 138 | return whiteFlag;
|
||
| 139 | } |
||
| 140 | |||
| 141 | |||
| 142 | /**
|
||
| 143 | * Pid controller which returns a corrections speed.
|
||
| 144 | */
|
||
| 145 | int LineFollow::getPidCorrectionSpeed(){
|
||
| 146 | c9fa414d | galberding | int32_t error = getError(); |
| 147 | int32_t sloap = oldError - error ; |
||
| 148 | 726fdc72 | galberding | // int correctionSpeed = (int) (global->K_p*error + Ki*accumHist - global->K_d*sloap);
|
| 149 | c9fa414d | galberding | int32_t correctionSpeed = (int32_t) (K_p*error + K_i*accumHist + K_d*sloap); |
| 150 | 726fdc72 | galberding | oldError = error; |
| 151 | c9fa414d | galberding | accumHist += error; |
| 152 | 726fdc72 | galberding | 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 | } |