void calibrateSensors() {

	unsigned int i=0;

	pwm_red = 0;
	pwm_green = 0;
	pwm_blue = 0;
	updateRedLed(pwm_red);
	updateGreenLed(pwm_green);
	updateBlueLed(pwm_blue);

	calibrationCycle = 0;
	startCalibration = 1;


	// calibrate accelerometer

	lastTick = getTime100MicroSec();
	while((getTime100MicroSec() - lastTick) < PAUSE_100_MSEC) {
		readAccelXYZ();	// get a fresh value from the accelerometer
	}
	
	accXMax = -1023;
	accXMin = 1023;
	accYMax = -1023;
	accYMin = 1023;
	accOffsetXSum = 0;
	accOffsetYSum = 0;

	if(abs(accZ) >= VERTICAL_THRESHOLD) {

		pwm_red = 0;
		pwm_green = 255;
		pwm_blue = 255;
		updateRedLed(pwm_red);
		updateGreenLed(pwm_green);
		updateBlueLed(pwm_blue);

		setLeftSpeed(0);
		setRightSpeed(0);

		while(1) {

			readAccelXYZ();

			handleMotorsWithNoController();

			if(calibrationCycle < CALIBRATION_CYCLES) {
				accOffsetXSum += accX;
				accOffsetYSum += accY;
				calibrationCycle++;
			} else {
				accOffsetX = accOffsetXSum>>4;
				accOffsetY = accOffsetYSum>>4;
				break;
			}

		}

	} else {
Exemplo n.º 2
0
// set speed for both motors
void ZumoMotors::setSpeeds(int leftSpeed, int rightSpeed)
{
  setLeftSpeed(leftSpeed);
  setRightSpeed(rightSpeed);
}
Exemplo n.º 3
0
/*
Permet de modifier la vitesse de moteur en choisisant la periode PWM du moteur gauche
*/
void ZumoMotors::setSpeeds(int leftSpeed, int rightSpeed, int periode)
{
  setLeftSpeedCurie(leftSpeed, periode);
  setRightSpeed(rightSpeed);
}
// set speed for both motors
void Zumo32U4Motors::setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
{
  setLeftSpeed(leftSpeed);
  setRightSpeed(rightSpeed);
}