Le code suivant contrôle les 2 moteurs de chaque jambe du robot bipède selon des consignes de position enregistrées dans un tableau du fichier Sequences.h que vous trouverez dans l’archive.
Contrairement à d’autres solutions, ici les servomoteurs sont contrôlés en microsecondes et pas en degrés : cela permet des mouvements beaucoup plus fluide, sans à-coups mais cela laisse moins de temps pour les autres activités d’un robot. Prenez ce critère en compte et optimisez vos algorithmes en fonction.
- //
- // on utilise la dernière version de la bibliothèque de contrôle de servomoteurs
- // il faut donc un environnement Arduino supérieur à la version 1.0
- #include <Servo.h>
- //
- // appel au fichier contenant le tableau de séquences
- #include "Sequences.h"
- //
- // deux variables pour gérer :
- // - le nombre de microsecondes d'incrément de la consigne du signal pour servomoteur
- // - le temps en microsecondes entre deux mises à jour des consignes
- #define INCREMENT 1
- #define DELAY_UPDATE 15
- //
- // ces servomoteurs ont un intervalle de commande entre 1000 et 2000 µs
- // on peut adapter ces chiffres selon le modèle
- #define SERVO_MIN 1000
- #define SERVO_MAX 2000
- //
- // les 4 instances de servomoteur sont stockées dans un tableau
- // pour faciliter la mise en adéquation avec les 4 valeurs dans le tableau de séquences
- Servo Servos[4];
- //
- // les dernières consignes à atteindre
- int consigne[4];
- //
- // les positions courantes données aux servomoteurs
- // car on ne passe pas instantanément d'une consigne à l'autre
- // de cette manière on contrôle la vitesse des moteurs et pas seulement leur position
- int position[4];
- // l'indice de la commande en cours dans la séquence en cours
- int index = 0;
- int ecart = 0;
- /**
- * Configuration de l'Arduino
- */
- void setup()
- {
- // les servomoteurs
- Servos[0].attach(6); // genou gauche
- Servos[1].attach(11); // hanche gauche
- Servos[2].attach(10); // genou droit
- Servos[3].attach(9); // hanche droite
- // il faut initialiser les variables pour éviter que le robot tombe au démarrage
- position[0] = 1500;
- position[1] = 1500;
- position[2] = 1500;
- position[3] = 1500;
- consigne[0] = 1500;
- consigne[1] = 1500;
- consigne[2] = 1500;
- consigne[3] = 1500;
- // position initiale (en degrés donc position milieu)
- Servos[0].write(90);
- Servos[1].write(90);
- Servos[2].write(90);
- Servos[3].write(90);
- }
- void loop()
- {
- updatePosition();
- delay(DELAY_UPDATE);
- }
- void updatePosition()
- {
- // on remet à 0 avant de mesurer l'écart entre la consigne et la position courante
- ecart = 0;
- for (int i = 0; i < 4 ; i++) {
- // ajout de l'écart du moteur à l'écart global
- ecart += abs(consigne[i]-position[i]);
- if (consigne[i] > position[i]+INCREMENT)
- {
- position[i] += INCREMENT;
- Servos[i].writeMicroseconds(position[i]);
- }
- if (consigne[i] < position[i] - INCREMENT)
- {
- position[i] -= INCREMENT;
- Servos[i].writeMicroseconds(position[i]);
- }
- }
- // on met à jour les consignes si la position des 4 servomoteurs est atteinte
- if (ecart < 4*INCREMENT) {
- index++;
- // si on dépasse, on revient au début du tableau
- if (index >= 10) index = 0;
- // les valeurs dans le tableau construit pas à pas en observant le robot
- // sont entre 1 et 9 pour être plus facile à comprendre donc on utilise une fonction
- // pour passer de l'intervalle "utilisateur" à l'intervalle "servomoteur"
- consigne[0] = map(sequences[index][0],1,9,SERVO_MIN,SERVO_MAX);
- consigne[1] = map(sequences[index][1],1,9,SERVO_MIN,SERVO_MAX);
- consigne[2] = map(sequences[index][2],1,9,SERVO_MIN,SERVO_MAX);
- consigne[3] = map(sequences[index][3],1,9,SERVO_MIN,SERVO_MAX);
- }
- }
Voici le contenu du fichier "Sequences.h" à créer à côte de votre sketch Arduino :
- /**
- * Séquence des valeurs de consignes pas à pas pour chaque moteur
- */
- // genou gauche / hanche gauche // genou droit / hanche droite
- int sequences[12][4] = {{5,5,5,5},{10,5,5,5},{9,5,8,5},{8,5,8,5},{8,3,8,3},{8,3,8,3},{5,3,5,3},{5,3,2,3},{4,3,0,3},{1,3,0,3},{1,5,3,5},{3,5,5,5}};