int
speedDifference
=
error
/ 4 + 6 *
lastError
=
error;
// Calculer la vitesse de chaque moteur. Le signe de la différence (speedDifference)
// determine si le moteur tourne à gauche ou a droite.
int
m1Speed
=
MAX_SPEED
+
speedDifference;
int
m2Speed
=
MAX_SPEED
-
speedDifference;
// Nous allons contraindre la vitesse des moteurs entre 0 et MAX_SPEED.
// D'une façon générale, un des moteurs est toujours à MAX_SPEED
// et l'autre sera à MAX_SPEED-|speedDifference| si elle est positif,
// sinon il sera en vitesse stationnaire. Pour certaines applications,
// vous pourriez désirer une vitesse négative, ce qui permettrai de
// tourner à l'envers.
if
(m1Speed
<
0)
m1Speed
=
0;
if
(m2Speed
<
0)
m2Speed
=
0;
if
(m1Speed
>
MAX_SPEED)
m1Speed
=
MAX_SPEED;
if
(m2Speed
>
MAX_SPEED)
m2Speed
=
MAX_SPEED;
motors.setSpeeds(m1Speed, m2Speed);
}
(error
-
lastError);