Revision 2330e415 devices/DiWheelDrive/userthread.cpp

View differences:

devices/DiWheelDrive/userthread.cpp
108 108
void copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
109 109
	target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL];
110 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]);
111
	// chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
112 112
}
113 113

  
114 114
// Fuzzyfication of the sensor values
......
311 311
	// Hard deviatio to right
312 312
	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY
313 313
		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){
314
			copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl);
314
			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
315 315
	// Hard deviation to left
316 316
	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY
317 317
		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){
318
			copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl);
319
	// Turn if white
318
			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
319
	// stop if white
320 320
	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE
321 321
		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE ){
322
			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
322
			copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
323 323
	}
324 324
}
325 325

  
......
452 452
                vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
453 453
                vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
454 454
            }
455
			lf.followLine(vcnl4020Proximity, &global);
455
			lf.followLine(vcnl4020Proximity, rpmFuzzyCtrl, &global);
456 456
            // chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
457 457
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
458 458
            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
......
461 461
	//if (configLineFollowing==2)
462 462
	//   lineFollownew
463 463
	//else
464
            lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl);
465
            setRpmSpeed(rpmFuzzyCtrl);
464
            // lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);
465
            // setRpmSpeed(rpmFuzzyCtrl);
466 466
        }
467 467

  
468
		// this->sleep(US2ST(5));
468 469
		this->sleep(CAN::UPDATE_PERIOD);
469 470
	}
470 471

  
471 472
  return RDY_OK;
472 473
}
473

  

Also available in: Unified diff