// — Programme Arduino —
// Trame de code générée par le générateur de code Arduino
// du site www.mon-club-elec.fr

// Auteur du Programme : X. HINAULT – Tous droits réservés
// Programme écrit le : 30/5/2011.

// ——- Licence du code de ce programme —–
//  This program is free software: you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation, either version 3 of the License,
//  or any later version.
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//  You should have received a copy of the GNU General Public License
//  along with this program.  If not, see <http://www.gnu.org/licenses/>.

// ////////////////////  PRESENTATION DU PROGRAMME ////////////////////

// ——– Que fait ce programme ? ———
 /* Quatre servomoteurs connectés à la carte Arduino sont contrôlés
à partir du PC

Controle par chaine de la forme : genouD(000) avec 000 = angle en degrés

Attention : trois chiffres obligatoires – pour 45 degrés on fera 045, pour -90, on fera -090

Les chaines reçues sur le port série décodées par le programme Arduino sont :
** pas(xxx) où xxx est le nombre de degrès à utiliser pour chaque cran des mouvements progressifs – 1 par défaut
** vitesse(xxx) où xxx est la vitesse en millisecondes (50 = lent, 10 = rapide, 20 = intermédiaire)
** reset() réinitialise la position des servomoteurs à la position initiale

** genouD(xxx) où xxx est l’angle absolu en degré – réalise le positionnement immédiat du servomoteur (brutal…)
** genouDto(xxx) où xxx est l’angle absolu en degré – réalise le positionnement progressif du servomoteur basé sur vitesse
** genouDtoR(xxx) où xxx est l’angle relatif en degré par rapport à la position courante – réalise le positionnement progressif du servomoteur basé sur vitesse

** Les fonctions sont dispo pour genouD, genouG, hancheD, hancheG

** Les valeurs négatives sont acceptées pour les angles relatifs

** genoux(xxx) où xxx est la valeur de l’angle absolu en degré – mouvement simultané des 2 genoux
** grandEcart(xxx) où xxx est la valeur de l’angle relatif à utiliser – mouvement opposé des 2 servomoteurs
** marcheAV(xxx) où xxx est le nombre de pas – réalise le nombre de pas en avant voulus

** servosRobotRSync(xxx, xxx, xxx, xxx) où xxx est la position relative de chaque servomoteur – réalise positionnement coordonné

*/

// — Fonctionnalités utilisées —

// Utilise / fonctionne avec une interface Processing coté PC
// Utilise 4 servomoteurs
// Utilise le stockage des variables en mémoire Flash Programme
// Utilise une fonction permettant de connaitre la RAM restant disponible

// ——– Circuit à réaliser ———

// ******* ATTENTION : il est possible de connecter directement 2 ou 3 servomoteurs sur la carte Arduino
// Connecter servomoteur Genou D sur broche 6
// Connecter servomoteur Hanche D sur broche 7
// Connecter servomoteur Hanche G sur broche 9
// Connecter servomoteur Genou G sur broche 8

// /////////////////////////////// 1. Entête déclarative ///////////////////////
// A ce niveau sont déclarées les librairies incluses, les constantes, les variables, les objets utiles…

// — Déclaration des constantes —

// — Inclusion des librairies —

#include <Servo.h> // librairie pour servomoteur

#include <Flash.h> // Inclusion librairie pour stockage en mémoire Flash Programme
// Avant utilisation, il faut installer manuellement cette librairie
// dans le dossier <Libraries> du dossier Arduino
// infos : www.mon-club-elec.fr/pmwiki_reference_arduino/pmwiki.php?n=Main.LibrairieFlashProgramme

// — Déclaration des constantes utiles —
const int APPUI=LOW; // constante pour tester état BP

//— Constantes utilisées avec le servomoteur
const int ANGLE_MIN=0; // angle position MIN en degrés
const int POS_MIN=550; // largeur impulsion pour position ANGLE_MIN degrés du servomoteur
                  // POS_MIN=550 pour ANGLE_MIN=0 avec un futaba S3003

const int ANGLE_MAX=172; // angle position MAX en degrés
int POS_MAX=2400; // largeur impulsion pour position ANGLE_MAX degrés du servomoteur
                  // POS_MAX=2400 pour ANGLE_MAX=172 pour futaba S3003

// pour étalonner un servomoteur, voir la page :
//http://www.mon-club-elec.fr/pmwiki_mon_club_elec/pmwiki.php?n=MAIN.ArduinoExpertSerieDepuisPCPositionServomoteur

// — Déclaration des constantes des broches E/S numériques —

const int brocheGenouD=6; // Constante broche
const int brocheHancheD=7; // Constante broche
const int brocheGenouG=8; // Constante broche
const int brocheHancheG=9; // Constante broche

// — Déclaration des constantes des broches analogiques —

// — Déclaration des variables globales —

 int octetReception=0; // variable de stockage des valeurs reçues sur le port Série
 long nombreReception=0; // variable de stockage du nombre  reçu sur le port Série
 long nombreReception0=0; // variable de stockage du dernier nombre  reçu sur le port Série
 String chaineReception=«  »; // déclare un objet String vide pour reception chaine

int valeur=0; // variable utile
long param=0; // paramètre transmis

long params[3]; // tableau de paramètres pour instructions à paramètres multiples

//—- initialisation valeur angles —

float angleGenouD=90; // variable de position du servo  en degrés
float angleGenouG=90; // variable de position du servo en degrés
float angleHancheD=90; // variable de position du servo en degrés
float angleHancheG=90; // variable de position du servo en degrés

float angleGenouD0=90; // variable de la dernière position du servo  en degrés
float angleGenouG0=90; // variable de la dernière position du servo  en degrés
float angleHancheD0=90; // variable de la dernière position du servo  en degrés
float angleHancheG0=90; // variable de la dernière position du servo  en degrés

int vitesse=20; // variable utilisée pour délai entre 2 positionnement servomoteur
int pas=20; // variable globale fixant nombre de pas pour positionnement progressif « to »

int compt=0; // variable util e
int pause=250; // variable utile

//float cran=1.0 // variable fixant la valeur cran progression positionnement relatif

// — Déclaration des objets utiles pour les fonctionnalités utilisées —

//— Création objet servomoteur
Servo servoGenouD;  // crée un objet servo pour contrôler le servomoteur
Servo servoGenouG;  // crée un objet servo pour contrôler le servomoteur
Servo servoHancheD;  // crée un objet servo pour contrôler le servomoteur
Servo servoHancheG;  // crée un objet servo pour contrôler le servomoteur

// ////////////////////////// 2. FONCTION SETUP = Code d’initialisation //////////////////////////
// La fonction setup() est exécutée en premier et 1 seule fois, au démarrage du programme

void setup()   { // debut de la fonction setup()

// — ici instructions à exécuter 1 seule fois au démarrage du programme —

// ——- Initialisation fonctionnalités utilisées ——-  

Serial.begin(115200); // initialise connexion série à 115200 bauds
// IMPORTANT : régler le terminal côté PC avec la même valeur de transmission

//— Initialisation Servomoteur
servoGenouD.attach(brocheGenouD);  // attache l’objet servo à la broche de commande du servomoteur
servoGenouG.attach(brocheGenouG);  // attache l’objet servo à la broche de commande du servomoteur
servoHancheD.attach(brocheHancheD);  // attache l’objet servo à la broche de commande du servomoteur
servoHancheG.attach(brocheHancheG);  // attache l’objet servo à la broche de commande du servomoteur

// ——- Broches en sorties numériques ——-  
 pinMode (brocheGenouD,OUTPUT); // Broche  configurée en sortie
 pinMode (brocheGenouG,OUTPUT); // Broche  configurée en sortie
 pinMode (brocheHancheG,OUTPUT); // Broche  configurée en sortie
 pinMode (brocheHancheD,OUTPUT); // Broche  configurée en sortie

// ——- Broches en entrées numériques ——-  

// ——- Activation si besoin du rappel au + (pullup) des broches en entrées numériques ——-  

// ——- Initialisation des variables utilisées ——-  

// ——- Codes d’initialisation utile ——-  

servoGenouD.writeMicroseconds(angle(angleGenouD)); // crée impulsion à partir valeur angle – plus précis que write()
servoGenouG.writeMicroseconds(angle(angleGenouG)); // crée impulsion à partir valeur angle – plus précis que write()
servoHancheD.writeMicroseconds(angle(angleHancheD)); // crée impulsion à partir valeur angle – plus précis que write()
servoHancheG.writeMicroseconds(angle(angleHancheG)); // crée impulsion à partir valeur angle – plus précis que write()

Serial.println(F(« Arduino OK »)); // debug

delay(2000);

//—– position de départ
servosRobot(95,95,90,85); // position des servos GD, HD, HG, GG de départ

//servosRobotRSync(90,0,0,-90); // positionnement synchronisé des servomoteurs

//servosRobotRSync(-90,0,0,90); // positionnement synchronisé des servomoteurs

//servosRobot( GD, HD, HG, GG ) ;

//— tests —
/*
//—– position de départ
servosRobot(95,95,90,85); // position des servos GD, HD, HG, GG de départ

//—- mise en appui sur pied gauche —-
servosRobotR(-30,0,0,0); // position relative des servos GD, HD, HG, GG

servosRobotRSync(-30,0,0,-35); // positionnement synchronisé des servomoteurs

servosRobotR(30,0,0,0); // position relative des servos GD, HD, HG, GG

// —- bassin droit vers avant —
servosRobotRSync(0,30,30,0); // positionnement synchronisé des servomoteurs

//—— pose pied droit —-
servosRobotRSync(30,0,0,35); // positionnement synchronisé des servomoteurs

//—- séquence de marche —

  delay(3000);
  vitesse=15; // variable utilisée pour délai entre 2 positionnement servomoteur
   reset();
   marcheAV(4); // appelle fonction marche AV

*/

} // fin de la fonction setup()
// ********************************************************************************

////////////////////////////////// 3. FONCTION LOOP = Boucle sans fin = coeur du programme //////////////////
// la fonction loop() s’exécute sans fin en boucle aussi longtemps que l’Arduino est sous tension

void loop(){ // debut de la fonction loop()

//—- code type réception chaine sur le port série —
while (Serial.available()>0) { // tant qu’un octet en réception
        octetReception=Serial.read(); // Lit le 1er octet reçu et le met dans la variable

        if (octetReception==10) { // si Octet reçu est le saut de ligne
                Serial.print (F(« Chaine recue= »)),Serial.print(chaineReception); // affiche la chaine recue

                analyseChaine(chaineReception); // appelle la fonction d’analyse de la chaine en réception

                chaineReception=«  »; //RAZ le String de réception
                break; // sort de la boucle while
        }
        else { // si le caractère reçu n’est pas un saut de ligne
                chaineReception=chaineReception+char(octetReception); // ajoute le caratère au String
        }

} // fin tant que  octet réception

//—– une fois que le saut de ligne est reçu, on sort du While et on se positionne ici

//chaineReception= » »;

//————- fin analyse chaine —————

delay(vitesse);

} // fin de la fonction loop() – le programme recommence au début de la fonction loop sans fin
// ********************************************************************************

// ////////////////////////// FONCTIONS DE GESTION DES INTERRUPTIONS ////////////////////

// ////////////////////////// AUTRES FONCTIONS DU PROGRAMME ////////////////////

//————- fonction calibrage impulsion servomoteur à partir valeur angle en degrés

//——- mieux avec float —–
float angle(float valeur_angle) {

        float impuls=0;
        impuls=map(valeur_angle,ANGLE_MIN,ANGLE_MAX,POS_MIN, POS_MAX);
        return impuls;  

} // fin fonction impulsion servomoteur

//————- fonction d’analyse de la chaine reçue sur le port série —-

//———— analyseChaine —————
void analyseChaine(String chaineRecue) { // fonction d’analyse de la chaine recue

  // —- analyse de la chaine recue sur le port Série —-
  chaineReception=chaineReception.trim(); // enlève les espaces

  //xxxxxxxxxxxxxxxxxxx instructions sans paramètres xxxxxxxxxxxx
 if (chaineReception==« reset() ») { // si instruction reçue
    reset(); // exécute instruction si valide
  }

  //xxxxxxxxxxxxxxxxxxxx instructions avec paramètres xxxxxxxxxxxxxxx

     // info : la valeur numérique extraite par testInstruction() est stockée dans variable globale param

  //================= instructions paramètres généraux =============

  //————– test instruction pas(xxx) ———–
  if (testInstruction(« pas(« )==true) { // si instruction reçue valide

    pas=param; // change valeur pas

    Serial.print(F(« pas = « )), Serial.println(pas);

  } // fin test pas(xxx)

  //————– test instruction vitesse(xxx) ———–
  if (testInstruction(« vitesse(« )==true) { // si instruction reçue valide

    vitesse=param; // change valeur vitesse (= durée delay en ms)
    Serial.print(F(« vitesse = « )), Serial.println(vitesse);

  } // fin test marcheAV(xxx)

  // ================ instructions fonctions de marche =============

    //————– test instruction marcheAV(xxx) ———–
  if (testInstruction(« marcheAV(« )==true) { // si instruction reçue valide

    marcheAV(param);
    Serial.print(F(« marche AV « )), Serial.print(param),Serial.println( » pas « );

  } // fin test marcheAV(xxx)

  //================ instructions figures ===============

    //————– test instruction grandEcart(xxx) ———–
  if (testInstruction(« grandEcart(« )==true) { // si instruction reçue valide

    //void servosRobotRSync( float GD, float HD, float HG, float GG) // reçoit les angles relatifs de Genou D, Hanche D, Hanche G, Genou G
    servosRobotRSync(param,0,0,-param); // positionnement synchronisé des servomoteurs

    Serial.print(F(« grand ecart »)), Serial.println(param);

  } // fin test grandEcart(xxx)

    //————— test instruction servosRobotRSync(xxx,xxx,xxx,xxx)

  if (testInstruction2(« servosRobotRSync(« ,4)==true) { // si instruction avec 4 paramètres reçue valide

    //void servosRobotRSync( float GD, float HD, float HG, float GG) // reçoit les angles relatifs de Genou D, Hanche D, Hanche G, Genou G
    servosRobotRSync(params[0],params[1],params[2],params[3]); // positionnement synchronisé des servomoteurs

    Serial.print(F(« servosRobotRSync(« )), Serial.print(params[0]),Serial.print(« , »), Serial.print(params[1]),Serial.print(« , »);
    Serial.print(params[2]),Serial.print(« , »), Serial.print(params[3]), Serial.print(F(« ) »));

  } // fin test grandEcart(xxx)

  //================ instructions genou droit =========

  //————– test instruction genouD(xxx) ———–
  if (testInstruction(« genouD(« )==true) { // si instruction reçue valide

    servoGenouD.writeMicroseconds(angle(param)); // crée impulsion à partir valeur angle – plus précis que write()

    angleGenouD0=param; // mémorise angle actuel

  } // fin test genouD(xxx)

  //————– test instruction genouDto(xxx) ———– // positionnement progressif
  if (testInstruction(« genouDto(« )==true) { // si instruction reçue valide

    // void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoTo( servoGenouD, angleGenouD0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleGenouD0=param; // met à jour l’angle courant servo  avec valeur extraite par testInstruction()

    Serial.print(F(« angleGenouD0 = « )), Serial.println(angleGenouD0);

  } // fin test genouDto(xxx)

  //————– test instruction genouDtoR(xxx) ———– // positionnement progressif
  if (testInstruction(« genouDtoR(« )==true) { // si instruction reçue valide

    // void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoToR( servoGenouD, angleGenouD0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleGenouD0=param+angleGenouD0; // met à jour l’angle courant servo  

    Serial.print(F(« angleGenouD0 = « )), Serial.println(angleGenouD0);

  } // fin test genouDto(xxx)

  //================ instructions genou gauche =========

  //————– test instruction genouG(xxx) ———–
  if (testInstruction(« genouG(« )==true) { // si instruction reçue valide

    servoGenouG.writeMicroseconds(angle(param)); // crée impulsion à partir valeur angle – plus précis que write()

    angleGenouG0=param; // mémorise angle actuel

  } // fin test genouG(xxx)

  //————– test instruction genouGto(xxx) ———– // positionnement progressif
  if (testInstruction(« genouGto(« )==true) { // si instruction reçue valide

    // void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoTo( servoGenouG, angleGenouG0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleGenouG0=param; // met à jour l’angle courant servo  avec valeur extraite par testInstruction()

    Serial.print(F(« angleGenouG0 = « )), Serial.println(angleGenouG0);

  } // fin test genouGto(xxx)

  //————– test instruction genouGtoR(xxx) ———– // positionnement progressif
  if (testInstruction(« genouGtoR(« )==true) { // si instruction reçue valide

    // void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoToR( servoGenouG, angleGenouG0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleGenouG0=param+angleGenouG0; // met à jour l’angle courant servo  

    Serial.print(F(« angleGenouG0 = « )), Serial.println(angleGenouG0);

  } // fin test genouGtoR(xxx)

  //================ instructions genoux =========

  //————– test instruction genoux(xxx) ———– // les 2 genoux en meme temps
  if (testInstruction(« genoux(« )==true) { // si instruction reçue valide

    servoGenouG.writeMicroseconds(angle(param)); // crée impulsion à partir valeur angle – plus précis que write()
    servoGenouD.writeMicroseconds(angle(param)); // crée impulsion à partir valeur angle – plus précis que write()

  } // fin test genoux(xxx)

  //================ instructions hancheD =========

  //————– test instruction hancheD(xxx) ———–
  if (testInstruction(« hancheD(« )==true) { // si instruction reçue valide

    servoHancheD.writeMicroseconds(angle(param)); // crée impulsion à partir valeur angle – plus précis que write()

  } // fin test hancheD(xxx)

  //————– test instruction hancheDto(xxx) ———– // positionnement progressif
  if (testInstruction(« hancheDto(« )==true) { // si instruction reçue valide

    // void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoTo( servoHancheD, angleHancheD0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleHancheD0=param; // met à jour l’angle courant servo  avec valeur extraite par testInstruction()

    Serial.print(F(« angleHancheD0 = « )), Serial.println(angleHancheD0);

  } // fin test hancheDto(xxx)

  //————– test instruction hancheDtoR(xxx) ———– // positionnement progressif
  if (testInstruction(« hancheDtoR(« )==true) { // si instruction reçue valide

    // void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoToR( servoHancheD, angleHancheD0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleHancheD0=angleHancheD0+param; // met à jour l’angle courant servo  

    Serial.print(F(« angleHancheD0 = « )), Serial.println(angleHancheD0);

  } // fin test hancheDtoR(xxx)

  //================ instructions hancheG =========

  //————– test instruction hancheG(xxx) ———–
  if (testInstruction(« hancheG(« )==true) { // si instruction reçue valide

    servoHancheG.writeMicroseconds(angle(param)); // crée impulsion à partir valeur angle – plus précis que write()

  } // fin test hancheG(xxx)

  //————– test instruction hancheGto(xxx) ———– // positionnement progressif
  if (testInstruction(« hancheGto(« )==true) { // si instruction reçue valide

    // void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoTo( servoHancheG, angleHancheG0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleHancheG0=param; // met à jour l’angle courant servo  avec valeur extraite par testInstruction()

    Serial.print(F(« angleHancheG0 = « )), Serial.println(angleHancheG0);

  } // fin test hancheDto(xxx)

  //————– test instruction hancheGtoR(xxx) ———– // positionnement progressif
  if (testInstruction(« hancheGtoR(« )==true) { // si instruction reçue valide

    // void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoToR( servoHancheG, angleHancheG0, param, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angleHancheG0=param+angleHancheG0; // met à jour l’angle courant servo  

    Serial.print(F(« angleHancheG0 = « )), Serial.println(angleHancheG0);

  } // fin test hancheDtoR(xxx)

} // —————- fin fonction analyse chaine ———————-

//————— testInstruction : test si instruction de la forme instruction(xxx) ————

boolean testInstruction(String chaineTest) { // reçoit chaine et renvoie true si instruction valide

  long posRef=chaineTest.length();// position de référence pour analyse (xxx)

  if (chaineReception.substring(0,posRef)==chaineTest) { // si reçoit l’instruction chaineTest(000)
  // nb substring : dernier caractere exclu

    Serial.print(F(« Arduino va executer : « ));

    Serial.print(chaineTest); // affiche

    if (chaineReception.substring(posRef,posRef+1)==« -« ) { // si signe – présent on décale de 1 position la prise en compte du nombre xxx

      param=(–1)*stringToLong(chaineReception.substring(posRef+1,posRef+4)); // extraction valeur 3 chiffres à partir caractères et * par -1
       posRef=posRef+1; // modif valeur posRef

    } // fin if

    else { // si pas de signe –

      param=stringToLong(chaineReception.substring(posRef,posRef+3)); // extraction valeur 3 chiffres à partir caractères

    } // fin else

    Serial.print(param); // affiche

    if (chaineReception.substring(posRef+3,posRef+4)==« ) ») { // si fermeture parenthèse = instruction valide

      Serial.println(F(« ) »)); // affiche
      Serial.println(F(« Instruction valide ! »)); // affiche
      return(true); // renvoie true si instruction valide

    } // fin si fermeture parenthèse

    else {

      Serial.println(F(« Instruction invalide ! »)); // affiche
      return(false); // renvoie false si instruction invalide

    } // fin else

  } // fin si reçoit l’instruction chaineTest(000)

} // fin fonction testInstruction  

//——————- fin test instruction ————

//————— testInstruction : test si instruction de la forme instruction(xxx, xxx, xxx, xxx) ou moins ————

boolean testInstruction2(String chaineTest, int nbParam) { // reçoit chaine  et renvoie true si instruction valide

  long posRef=chaineTest.length();// position de référence pour analyse (xxx)

  if (chaineReception.substring(0,posRef)==chaineTest) { // si reçoit l’instruction chaineTest(000)
  // nb substring : dernier caractere exclu

    Serial.print(F(« Arduino va executer : « ));

    Serial.print(chaineTest); // affiche

    for (int i; i<nbParam; i++) { // défile les n paramètres

        if (chaineReception.substring(posRef,posRef+1)==« -« ) { // si signe – présent on décale de 1 position la prise en compte du nombre xxx

          posRef=posRef+1; // modif valeur posRef
          params[i]=(–1)*stringToLong(chaineReception.substring(posRef,posRef+3)); // extraction valeur 3 chiffres à partir caractères et * par -1

        } // fin if

        else { // si pas de signe –

          params[i]=stringToLong(chaineReception.substring(posRef,posRef+3)); // extraction valeur 3 chiffres à partir caractères

        } // fin else

        Serial.print(params[i]); // affiche

        if (chaineReception.substring(posRef+3,posRef+4)==« , ») { // si parenthèse attendue présente

          Serial.print(« , »); // affiche
          posRef=posRef+4; //décale position de référence de 4 caractères pour prise en compte nouvelle valeur

        } // fin if « , »

    } // fin for nbParam

    if (chaineReception.substring(posRef+3,posRef+4)==« ) ») { // si fermeture parenthèse = instruction valide

      Serial.println(F(« ) »)); // affiche
      Serial.println(F(« Instruction valide ! »)); // affiche
      return(true); // renvoie true si instruction valide

    } // fin si fermeture parenthèse

    else {

      Serial.println(F(« Instruction invalide ! »)); // affiche
      return(false); // renvoie false si instruction invalide

    } // fin else

  } // fin si reçoit l’instruction chaineTest(000)

} // fin fonction testInstruction2  

//——————- fin test instruction2 ————

// ———- fonction de conversion d’un String numérique en long

long stringToLong(String chaineLong) { // fonction conversion valeur numérique String en int

    long nombreLong=0; // variable locale
    int valeurInt=0; // variable locale

    for (int i=0; i<chaineLong.length(); i++) { // défile caractères de la chaine numérique

      valeurInt=chaineLong.charAt(i); // extrait le caractère ASCII à la position voulue – index 0 est le 1er caractère
      valeurInt=valeurInt–48; // obtient la valeur décimale à partir de la valeur ASCII  

     if (valeurInt>=0 && valeurInt<=9) { // si caractère est entre 0 et 9
       nombreLong=(nombreLong*10)+valeurInt;
     } // fin si caractère est entre 0 et 9

    } // fin for défile caractères

 return (nombreLong); // renvoie valeur numérique

} // ———- fin stringToLong ————

//— fonction de positionnement progressif du servomoteur par pas fixe  —–

 void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas) {

       //— positionnement progressif par pas fixe de 1 degré —

    int delta=toAngle–fromAngle; // variation d’angle

    Serial.print(F(« delta = « )), Serial.println(delta);

    if (delta>=0) { // si variation positive

      for (int i=0; i<delta; i++) { // defile n positions pour atteindre angle final dans sens positif

        fromAngle=fromAngle+1; // ajoute cran
        toServo.writeMicroseconds(angle(fromAngle)); // crée impulsion à partir valeur angle – plus précis que write()
        //Serial.print(« angle courant servo = « ), Serial.println(fromAngle);
        delay(vitesse); // pause entre chaque positionnement

      } // fin for

    } // fin if

    else { // si variation négative

      for (int i=-delta; i>0; i—) { // defile n positions pour atteindre angle final dans sens négatif

        fromAngle=fromAngle–1; // ajoute cran
        toServo.writeMicroseconds(angle(fromAngle)); // crée impulsion à partir valeur angle – plus précis que write()
        //Serial.print(« angle courant servo = « ), Serial.println(fromAngle);
        delay(vitesse); // pause entre chaque positionnement

      } // fin for

    } // fin else

 }

//— fonction de positionnement progressif du servomoteur par pas fixe – angle relatif à la position courante  —–

 void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas) {

       //— positionnement progressif par pas fixe de 1 degré —

    int delta=toAngle; // variation d’angle correspond à l’angle transmis

    Serial.print(F(« delta = « )), Serial.println(delta);

    if (delta>=0) { // si variation positive

      for (int i=0; i<delta; i++) { // defile n positions pour atteindre angle final dans sens positif

        fromAngle=fromAngle+1; // ajoute cran
        toServo.writeMicroseconds(angle(fromAngle)); // crée impulsion à partir valeur angle – plus précis que write()
        //Serial.print(« angle courant servo = « ), Serial.println(fromAngle);
        delay(vitesse); // pause entre chaque positionnement

      } // fin for

    } // fin if

    else { // si variation négative

      for (int i=-delta; i>0; i—) { // defile n positions pour atteindre angle final dans sens négatif

        fromAngle=fromAngle–1; // ajoute cran
        toServo.writeMicroseconds(angle(fromAngle)); // crée impulsion à partir valeur angle – plus précis que write()
        //Serial.print(« angle courant servo = « ), Serial.println(fromAngle);
        delay(vitesse); // pause entre chaque positionnement

      } // fin for

    } // fin else

 }

//———— positionnement des servomoteurs du robot ——
void servosRobot( float GD, float HD, float HG, float GG) { // reçoit les angles de Genou D, Hanche D, Hanche G, Genou G

  // void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)

  //Genou Droit
  servoTo( servoGenouD, angleGenouD0, GD, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —
  angleGenouD0=GD; // mise à jour angle

  //Hanche Droite
  servoTo( servoHancheD, angleHancheD0, HD, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —
  angleHancheD0=HD; // mise à jour angle

  //Hanche Gauche
  servoTo( servoHancheG, angleHancheG0, HG, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —
  angleHancheG0=HG; // mise à jour angle

  //Genou Gauche
  servoTo( servoGenouG, angleGenouG0, GG, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —
  angleGenouG0=GG; // mise à jour angle

} // fin servosRobot

//———— positionnement des servomoteurs du robot en poistion relative ——
void servosRobotR( float GD, float HD, float HG, float GG) { // reçoit les angles relatifs de Genou D, Hanche D, Hanche G, Genou G

  // void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)

  //Genou Droit
  servoToR( servoGenouD, angleGenouD0, GD, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —
  angleGenouD0=angleGenouD0+GD; // mise à jour angle

  //Hanche Droite
  servoToR( servoHancheD, angleHancheD0, HD, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —
  angleHancheD0=angleHancheD0+HD; // mise à jour angle

  //Hanche Gauche
  servoToR( servoHancheG, angleHancheG0, HG, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —
  angleHancheG0=angleHancheG0+HG; // mise à jour angle

  //Genou Gauche
  servoToR( servoGenouG, angleGenouG0, GG, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —
  angleGenouG0=angleGenouG0+GG; // mise à jour angle

} // fin servosRobot

//———— fonction de positionnement synchronisé des servomoteurs du robot en poistion relative ——
void servosRobotRSync( float GD, float HD, float HG, float GG) { // reçoit les angles relatifs de Genou D, Hanche D, Hanche G, Genou G

  //——— calcul de la variation d’angle pour chaque servomoteur
  float deltaGD=GD; // genou Droit
  float deltaHD=HD; // hanche Droit
  float deltaHG=HG; // hanche gauche
  float deltaGG=GG; // genou gauche

  //——— calcul des valeur absolues des delta ———-
  //— calculé ici pour éviter d’utiliser fonctions dans fonction max() – cf Référence
  float absDeltaGD=abs(deltaGD);
  float absDeltaHD=abs(deltaHD);
  float absDeltaHG=abs(deltaHG);
  float absDeltaGG=abs(deltaGG);

  //—— calcul du deltaMax = la plus grande valeur absolue des delta
  float deltaMax=0;

  deltaMax=max(deltaMax, absDeltaGD);
  deltaMax=max(deltaMax, absDeltaHD);
  deltaMax=max(deltaMax,absDeltaHG);
  deltaMax=max(deltaMax, absDeltaGG);

  Serial.print(F(« deltaMax = « )), Serial.println(deltaMax);

  // ———- calcul des crans d’incrémentation pour chaque servomoteur —-
  //— utilise delta avec signe +/- —

  float cranGD=deltaGD/deltaMax; // divise delta / nombre de pas à effectuer
  float cranHD=deltaHD/deltaMax; // divise delta / nombre de pas à effectuer
  float cranHG=deltaHG/deltaMax; // divise delta / nombre de pas à effectuer
  float cranGG=deltaGG/deltaMax; // divise delta / nombre de pas à effectuer

  Serial.print(F(« cranGD = « )), Serial.println(cranGD);
  Serial.print(F(« cranHD = « )), Serial.println(cranHD);
  Serial.print(F(« cranHG = « )), Serial.println(cranHG);
  Serial.print(F(« cranGG = « )), Serial.println(cranGG);

  //——– réinitialise variable angle courant des servomoteurs —-
  //- évite de modifier angleGenouD0 lors des calculs
  angleGenouD=angleGenouD0;
  angleHancheD=angleHancheD0;
  angleHancheG=angleHancheG0;
  angleGenouG=angleGenouG0;

  //———– défile les deltaMax positions et positionne les servomoteurs ————–
  for (int i=0; i<deltaMax; i++) {

        //———- servomoteur Genou Droite
        angleGenouD=angleGenouD+cranGD; // ajoute cran
        servoGenouD.writeMicroseconds(angle(angleGenouD)); // crée impulsion à partir valeur angle – plus précis que write()
        Serial.print(F( » / GD = « )), Serial.print(angleGenouD);

        //———- servomoteur Hanche Droite
        angleHancheD=angleHancheD+cranHD; // ajoute cran
        servoHancheD.writeMicroseconds(angle(angleHancheD)); // crée impulsion à partir valeur angle – plus précis que write()
        Serial.print(F( » / HD = « )), Serial.print(angleHancheD);

        //———- servomoteur Hanche Gauche
        angleHancheG=angleHancheG+cranHG; // ajoute cran
        servoHancheG.writeMicroseconds(angle(angleHancheG)); // crée impulsion à partir valeur angle – plus précis que write()
        Serial.print(F( » / HG = « )), Serial.print(angleHancheG);

        //———- servomoteur Genou Gauche
        angleGenouG=angleGenouG+cranGG; // ajoute cran
        servoGenouG.writeMicroseconds(angle(angleGenouG)); // crée impulsion à partir valeur angle – plus précis que write()
        Serial.print(F( » / GG = « )), Serial.print(angleGenouG);

        //————– pause vitesse entre 2 positionnement des servomoteurs pour mouvement progressif
        Serial.println();
        delay(vitesse); // pause après positionnement de tous les servomoteurs

  } // fin for deltaMax

    //————- mise à jour variable des angles courants ——————  
     //— en se basant sur valeur angle de départ et delta —
    //— le résultat doit correspondre à celui obtenu par calculs précédents

    angleGenouD0=angleGenouD0+GD; // genou Droit
    angleHancheD0=angleHancheD0+HD; // hanche Droit
    angleHancheG0=angleHancheG0+HG; // hanche gauche
    angleGenouG0=angleGenouG0+GG; // genou gauche

    Serial.print(F(« / GD0 = « )), Serial.print(angleGenouD0);
    Serial.print(F(« / HD0 = « )), Serial.print(angleHancheD0);
    Serial.print(F(« / HG0 = « )), Serial.print(angleHancheG0);
    Serial.print(F(« // GG0 = « )), Serial.print(angleGenouG0);

    Serial.println();

} // fin servosRobotRSync – fonction de positionnement synchronisé

// ////////////////////////// Fin du programme ////////////////////

//———— fonction Reset —————

void reset(void) {

servoGenouD.writeMicroseconds(angle(90)); // crée impulsion à partir valeur angle – plus précis que write()
servoGenouG.writeMicroseconds(angle(90)); // crée impulsion à partir valeur angle – plus précis que write()
servoHancheD.writeMicroseconds(angle(90)); // crée impulsion à partir valeur angle – plus précis que write()
servoHancheG.writeMicroseconds(angle(90)); // crée impulsion à partir valeur angle – plus précis que write()

angleGenouD0=90;
angleHancheD0=90;
angleHancheG0=90;
angleGenouG0=90;

Serial.println(F(« Reset OK »)); // debug

delay(2000);

} // fin reset

// ————– fonction marche AV ————

void marcheAV(int nbPas) {

  for (int i=0; i<nbPas; i++) { // répète le nombre de pas voulus  

    sequence1();

  } // fin for

} // fin fonction marcheAV

//—— séquence 1 – marche AV —

void sequence1 (void) {

  // xxxxxxxxxxxxxxx séquence pas mal xxxxxxxxxxxxxxx

//—– position de départ
servosRobot(95,95,90,85); // position des servos GD, HD, HG, GG de départ

//—- mise en appui sur pied gauche —-

servosRobotR(–30,0,0,0); // position relative des servos GD, HD, HG, GG

servosRobotR(0,0,0,-15); // position relative des servos GD, HD, HG, GG

servosRobotR(–30,0,0,0); // position relative des servos GD, HD, HG, GG

servosRobotR(0,0,0,-20); // position relative des servos GD, HD, HG, GG

servosRobotR(30,0,0,0); // position relative des servos GD, HD, HG, GG

// —- bassin droit vers avant —
servosRobotR(0,30,30,0); // position relative des servos GD, HD, HG, GG

//—— pose pied droit —-
servosRobotR(0,0,0,35); // position relative des servos GD, HD, HG, GG

servosRobotR(30,0,0,0); // position relative des servos GD, HD, HG, GG

//——- mise en appui sur pied droit —

servosRobotR(0,0,0,30); // position relative des servos GD, HD, HG, GG

servosRobotR(15,0,0,0); // position relative des servos GD, HD, HG, GG

servosRobotR(0,0,0,30); // position relative des servos GD, HD, HG, GG

servosRobotR(20,0,0,0); // position relative des servos GD, HD, HG, GG

servosRobotR(0,0,0,-30); // position relative des servos GD, HD, HG, GG

//——– rotation bassin gauche vers avant —-
servosRobotR(0,-30,-30,0); // position relative des servos GD, HD, HG, GG

// —- pose pied gauche —–

servosRobotR(–35,0,0,0); // position relative des servos GD, HD, HG, GG

servosRobotR(0,0,0,-30); // position relative des servos GD, HD, HG, GG

}