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 {
// set speed for both motors
void ZumoMotors::setSpeeds(int leftSpeed, int rightSpeed)
{
setLeftSpeed(leftSpeed);
setRightSpeed(rightSpeed);
}
/*
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);
}