Pololu Zumo Guide Utilisateur page 44

waitForButtonAndCountDown(true);
}
loop_start_time = millis();
lsm303.readAcceleration(loop_start_time);
sensors.read(sensor_values);
if
((_forwardSpeed == FullSpeed) &&
{
setForwardSpeed(SustainedSpeed);
}
if
(sensor_values[0] < QTR_THRESHOLD)
{
// Si le senseur le plus à gauche détecte une ligne, marche arrière et tourner à droite
turn(RIGHT, true);
}
else if
(sensor_values[5] < QTR_THRESHOLD)
{
// Si le senseur le plus a droite détecte une ligne, marche arrière et tourner à gauche
turn(LEFT, true);
}
else // Sinon, aller tout droit
{
// vérifier si contact; signaler contact
if
(check_for_contact()) on_contact_made();
int speed = getForwardSpeed();
motors.setSpeeds(speed, speed);
}
}
// Exécuter une rotation
// direction: RIGHT (droite) ou LEFT (gauche)
// Angle aléatoire pour améliorer la recherche
void turn(char direction, bool randomize)
{
#ifdef LOG_SERIAL
Serial.print("turning ...");
Serial.println();
#endif
// assume qu'il n'y a pas de contact (pas de collision).
on_contact_lost();
static
unsigned int duration_increment
// motors.setSpeeds(0,0);
// delay(STOP_DURATION);
motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
delay(REVERSE_DURATION);
motors.setSpeeds(TURN_SPEED *
delay(randomize ? TURN_DURATION
int speed = getForwardSpeed();
motors.setSpeeds(speed, speed);
last_turn_time = millis();
}
void setForwardSpeed(ForwardSpeed speed)
{
_forwardSpeed = speed;
if
(speed == FullSpeed) full_speed_start_time
}
int getForwardSpeed()
{
int speed;
switch (_forwardSpeed)
{
case
FullSpeed:
speed = FULL_SPEED;
break;
case SustainedSpeed:
speed = SUSTAINED_SPEED;
break;
default:
speed = SEARCH_SPEED;
break;
}
return speed;
}
// Vérifie s'il y a une collision/contact, mais ignore les lectures immédiatement après une rotation ou une perte de contact
bool check_for_contact()
{
static
long threshold_squared = (long) XY_ACCELERATION_THRESHOLD
return (lsm303.ss_xy_avg() > threshold_squared)
(loop_start_time - last_turn_time >
(loop_start_time - full_speed_start_time
= TURN_DURATION / 4;
direction, -TURN_SPEED * direction);
+ (random(8) - 2) * duration_increment
= loop_start_time;
&& \
MIN_DELAY_AFTER_TURN) && \
> FULL_SPEED_DURATION_LIMIT))
: TURN_DURATION);
* (long) XY_ACCELERATION_THRESHOLD;