Logiciel d’un robot bipède

//
// 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);
  }

}

/** 
 * 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}};