Les robots bipèdes sont des machines qui peuvent se déplacer et effectuer des tâches complexes. Ils sont de plus en plus utilisés dans des applications industrielles et médicales. La technologie de contrôle des robots bipèdes est en constante évolution et de nouvelles méthodes sont développées pour améliorer leur fonctionnement. Une de ces méthodes est le contrôle par chaines de caractères depuis le Terminal Série. Cette technologie permet aux utilisateurs de contrôler leurs robots bipèdes à distance et de leur donner des instructions précises. Dans cet article, nous allons examiner en détail le contrôle par chaines de caractères depuis le Terminal Série et ses avantages et inconvénients.
Robot Bipède : Contrôle par chaines de caractères depuis le Terminal Série
1. Présentation
Ce programme permet de contrôler le robot bipède par réception de chaines de caractères en provenance du Terminal Série via le port série USB de la carte Arduino. Ainsi, les quatre servomoteurs connectés à la carte Arduino sont contrôlés à partir du PC
Le contrôle du robot bipède se fait 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é
Ce programme utilise les fonctionnalités suivantes :
- Utilise la connexion série vers le PC
- Utilise 4 servomoteurs
Ressources utiles associées à ce programme :
- La librairie Serial – pour les communications séries entre la carte Arduino et l’ordinateur ou d’autres composants
- La librairie Servo – pour contrôler les servomoteurs.
- La librairie Flash – hors référence – pour mettre simplement les chaînes de caractères et constantes de vos programmes dans la mémoire Flash programme de votre carte Arduino.
2. Matériel Nécessaire
2.1 L’espace de développement Arduino
- … pour éditer, compiler le programme et programmer la carte Arduino.

2.2 Le matériel électronique suivant pour réaliser le montage associé
- quatre servomoteurs standards type Futaba S3003




- Pour étalonner un servomoteur dont on ne connaît pas les caractéristiques, voir le page : http://www.mon-club-elec.fr/pmwiki_mon_club_elec/pmwiki.php?n=MAIN.ArduinoExpertSerieDepuisPCPositionServomoteur
- Idéalement, un Shield kit EasyCard pour utilisation simplifiée jusqu’à 20 servos et/ou 6 capteurs analogiques – Easyrobotics

2.3 Les pièces mécaniques suivantes
- Un kit robot bipède de chez http://www.easyrobotics.fr comportant :
- 4 cages Easy
- 2 pieds
- 2 fourches pour bras pour la jonction hanche/genou
- 1 plateau bassin
- 2 plaques Avant/arrière de bassin
3. Instructions de montage
- La connexion série vers le PC utilise les broches 0 et 1 (via le câble USB)
- Enficher la EasyCard broche à broche 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
- Dans le cas d’une carte Arduino :
- l’intensité maximale disponible sur une broche est de 40mA
- l’intensité maximale cumulée pour l’ensemble des broches est 200mA
- l’instensité maximale que peut fournir l’alimentation 5V de la carte est 500mA.
- Par conséquent, avec une carte Arduino :
- En ce qui concerne la ligne de commande du servomoteur par la carte Arduino :
- on pourra commander directement autant de servomoteur que l’on veut (broche de commande) avec une carte Arduino, le nombre maximal étant 200mA / I commande, soit 100 environ dans le cas du Futaba S3003, ce qui est largement supérieur au nombre de broches de la carte.
- Il n’y aura par ailleurs pas besoin d’amplification (type ULN 2803) sur la ligne de commande du servomoteur même si l’on utilise un grand nombre de servomoteurs.
- En ce qui concerne l’alimentation principale des servomoteurs par une carte Arduino
- on ne peut alimenter que 3 à 4 servomoteurs simultanément par l’alimentation 5V de la carte Arduino, le nombre maximal étant 500mA / I fonctionnement = 500 / 120 = 4 servomoteurs dans le cas du Futaba S3003.
- Une alimentation externe sera indispensable dès que l’on dépassera ce nombre pour ne pas risquer de « griller » la carte Arduino.
- En ce qui concerne la ligne de commande du servomoteur par la carte Arduino :
Pour plus de détails, voir : Principes d’utilisation des alimentations avec une carte Arduino et des servomoteurs
4. Le schéma théorique du montage
Le schéma théorique du montage (cliquer pour agrandir)
5. Le circuit du montage
Le schéma du montage à réaliser (cliquer pour agrandir)
6. Explication du programme
- Le programme écoute le port série : lorsqu’une chaîne est reçue, elle est analysée et lorsqu’elle correspond à une chaine attendue, la fonction voulue de positionnement des servomoteurs est exécutée.
- A noter que le programme est capable de recevoir une instruction à 4 paramètres sur le port série et d’en extraire correctement les 4 valeurs numériques voulues.
- Le programme comprend plusieurs fonctions de positionnement des servomoteurs :
- de façon progressive, en angle absolu ou relatif
- de façon synchronisée pour plusieurs servomoteurs simultanément, là encore en angle absolu ou relatif.
- Voir les commentaires du programme pour plus de détails.
7. Mise en oeuvre du programme
7.1 Préparation du montage et programmation de la carte Arduino :
- Commencer par réaliser le montage indiqué sur plaque d’expérimentation
- Ensuite, programmer la carte Arduino avec ce programme (en bas de page) selon la procédure habituelle
7.2 Préparation du Terminal côté PC dans le logiciel Arduino
- Côté PC, il faut ouvrir la fenêtre terminal de l’IDE Arduino : pour ce faire, un simple clic sur le bouton « Sérial Monitor ».

- La fenêtre « Terminal » s’ouvre alors :

- Il faut alors régler le débit de communication sur la même valeur que celle utilisée par le programme avec lequel nous allons programmer la carte Arduino :

7.3 Fonctionnement
Le contrôle du robot bipède se fait 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é
8. Le programme complet en langage Arduino
A copier/coller directement dans l’éditeur 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
}
Articles similaires:
- http://web.archive.org/web/20210804223007/http://www.mon-club-elec.fr/pmwiki_mon_club_elec/pmwiki.php?n=MAIN.ArduinoExpertMoteursServo
- Bras robotisé 5 servomoteurs avec pince : Test de préhension de balle entre 2 position et contrôle par chaines de caractères depuis le Terminal Série
- http://web.archive.org/web/20210804223007/http://www.mon-club-elec.fr/pmwiki_mon_club_elec/pmwiki.php?n=MAIN.ATELIERSMoteursServosStandards
- Test simple d’un servomoteur à rotation continue : contrôle de la vitesse et du sens de rotation à l’aide d’une résistance variable.
- http://web.archive.org/web/20210804223007/http://www.mon-club-elec.fr/pmwiki_mon_club_elec/pmwiki.php?n=MAIN.ArduinoExpertMotorisations
Articles Liés
- Javascript : Graphique Dygraphs simple
Le Javascript est un langage de programmation très populaire et puissant qui permet aux développeurs…
- Javascript : Afficher 6 widgets graphiques fournis par une librairie graphique externe.
Le Javascript est un langage de programmation très populaire qui permet aux développeurs de créer…
- Javascript : Graphique Dygraphs : afficher date à partir unixtime
Le langage de programmation Javascript est très populaire et est utilisé pour créer des applications…