Logo Mon Club Elec

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

Les robots sont de plus en plus présents dans notre quotidien et leur utilisation s’étend à de nombreux domaines. Dans ce contexte, le bras robotisé 5 servomoteurs avec pince est un outil très intéressant pour les applications industrielles et scientifiques. Dans cet article, nous allons tester la préhension de balle entre 2 positions et le contrôle par chaines de caractères depuis le Terminal Série. Nous verrons comment le bras robotisé 5 servomoteurs avec pince peut être utilisé pour effectuer ces tâches et comment il peut être contrôlé à distance.

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

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

1.  Présentation

Ce programme permet de contrôler le bras articulé à 5 servomoteurs (1 servo base tournante, 3 servos de flexion et 1 servo de pince) 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 cinq servomoteurs connectés à la carte Arduino sont contrôlés à partir du PC.

Un code d’exemple permet de tester la préhension d’une balle entre 2 positions.

Le contrôle du bras articulé à 5 servomoteurs se fait par chaine de la forme : servoto(i,xxx) avec xxx = angle en degrés.

Le code présenté ici utilise de nombreux tableaux avec une désignation des servomoteurs par indice : le code sera donc facilement transposable pour être utilisé avec 12, 20 servomoteurs ou plus !

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 :

  • reset() : réinitialise position initiale du bras
  • vitesse(xxx) : paramétrage vitesse en ms – rapide = 5ms, lent 50ms, moyen 20ms
  • pas(xxx) : pas de psoitionnement en degrés. 1 degré par défaut
  • servo(i,xxx) : positionnement brut (et brutal…) d’un servomoteur à la valeur angle absolu
  • servoto(i,xxx) : positionnement progressif angle absolu
  • servotoR(i,xxx) : positionnement progressif angle relatif, valeur négative acceptée
  • servosBrasRSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle relatif – valeur négative acceptée
  • servosBrasSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle absolu

NB : Pendant l’exécution de ces 2 fonctions, tous les angles intermédiaires des servomoteurs sont affichés dans le Terminal !

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

Ce programme utilise les fonctionnalités suivantes :

  • Utilise la connexion série vers le PC
  • Utilise 5 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.
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

2.2  Le matériel électronique suivant pour réaliser le montage associé

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 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 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 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
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

2.3  Les pièces mécaniques suivantes

  • Un kit bras robotisé 5 servomoteurs avec pince de chez http://www.easyrobotics.fr comportant :
    • 4 cages Easy standard + 1 grande cage Easy avec servomoteurs
    • 1 base pour bras avec plateau tournant
    • 1 rallonge bras
    • 1 pince avec engrenage et mords.

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
  • Connexion des servomoteurs :
    • connecter servo[0] = rotation base du bras sur la broche 6
    • connecter servo[1] = inclinaison base du bras sur la broche 7
    • connecter servo[2] = articulation médiane du bras sur la broche 8
    • connecter servo[3] = inclinaison pince du bras sur la broche 9
    • connecter servo[4] = ouverture pince du bras sur la broche 10

On utilise ici 5 servomoteurs dont un gros servomoteur pour l’axe de base du bras : IL EST IMPERATIF D’UTILISER UNE ALIMENTATION EXTERNE 5V pouvant supporter plusieurs ampères.
Il faudra également toujours vérifier que cette alimentation est allumée avant de connecter le port USB de la carte Arduino, sinon c’est le port USB qui alimentera les servomoteurs… avec un risque de détérioration, bien que la carte Arduino dispose d’une protection par fusible intégré

  • 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.

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

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

Le schéma théorique du montage (cliquer pour agrandir)

5.  Le circuit du montage

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

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 à 5 paramètres sur le port série et d’en extraire correctement les 5 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 ».
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
  • La fenêtre « Terminal » s’ouvre alors :
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
  • 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 :
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

7.3  Fonctionnement

Le contrôle du bras articulé à 5 servomoteurs se fait par chaine de la forme : servoto(i,xxx) avec xxx = angle en degrés.

Le code présenté ici utilise de nombreux tableaux avec une désignation des servomoteurs par indice : le code sera donc facilement transposable pour être utilisé avec 12, 20 servomoteurs ou plus !

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 :

  • reset() : réinitialise position initiale du bras
  • vitesse(xxx) : paramétrage vitesse en ms – rapide = 5ms, lent 50ms, moyen 20ms
  • pas(xxx) : pas de psoitionnement en degrés. 1 degré par défaut
  • servo(i,xxx) : positionnement brut (et brutal…) d’un servomoteur à la valeur angle absolu
  • servoto(i,xxx) : positionnement progressif angle absolu
  • servotoR(i,xxx) : positionnement progressif angle relatif, valeur négative acceptée
  • servosBrasRSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle relatif – valeur négative acceptée
  • servosBrasSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle absolu

NB : Pendant l’exécution de ces 2 fonctions, tous les angles intermédiaires des servomoteurs sont affichés dans le Terminal !

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

Se reporter au programme pour plus de détails.

8.  Le programme complet en langage Arduino

A copier/coller directement dans l’éditeur Arduino

// — 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 : 25/06/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 ? ———
 /* Cinq servomoteurs connectés à la carte Arduino sont contrôlés
à partir du PC (interface Processing)

servo[0] = rotation base du bras
servo[1] = inclinaison base du bras
servo[2] = articulation médiane du bras
servo[3] = inclinaison pince du bras
servo[4] = ouverture pince du bras

Controle par chaine de la forme : servo(i,000) avec i = indice du servomoteur et 000 = angle en degrés

== Fonctions disponibles en saisie depuis le Terminal série pour le controle du bras :

reset() : réinitialise position initiale du bras

vitesse(xxx) : paramétrage vitesse en ms – rapide = 5ms, lent 50ms, moyen 20ms
pas(xxx) : pas de psoitionnement en degrés. 1 degré par défaut

servo(i,xxx) : positionnement brut (et brutal…) d’un servomoteur à la valeur angle absolu
servoto(i,xxx) : positionnement progressif angle absolu
servotoR(i,xxx) : positionnement progressif angle relatif, valeur négative acceptée

servosBrasRSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle relatif
servosBrasSync(xxx,xxx,xxx,xxx,xxx); positionnement synchronisé en angle absolu

// pendant l’exécution de ces 2 fonctions, tous les angles intermédiaires des servomoteurs sont affichés dans le Terminal !

NB : les fonctions saisies depuis la fenetre Terminal supportent les valeurs négatives
NB2 : les valeurs numériques doivent etre saisies avec 3 chiffres – mettre 0 devant au besoin

*/

// — Fonctionnalités utilisées —

// Utilise / fonctionne avec une interface Processing coté PC
// Utilise 5 servomoteurs
// Utilise le stockage des variables en mémoire Flash Programme

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

// ******* ATTENTION : il est possible de connecter directement 2 ou 3 servomoteurs sur la carte Arduino (500mA maxi)
// au-delà utiliser une alimentation externe 5V

// connecter servo[0] = rotation base du bras sur la broche 6
// connecter servo[1] = inclinaison base du bras sur la broche 7
// connecter servo[2] = articulation médiane du bras sur la broche 8
// connecter servo[3] = inclinaison pince du bras sur la broche 9
// connecter servo[4] = ouverture pince du bras sur la broche 10

// /////////////////////////////// 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 :
//https://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 nbServos=5; // constante du nombre de servomoteurs

const int brocheServo[nbServos]={6,7,8,9,10}; // Tableau de constantes de broche

const float angleInitServo[nbServos]={90,120,10,10,45}; // tableau de constante de position initiale du servo  en degrés

// — 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[5]; // tableau de paramètres pour instructions à paramètres multiples

//—- initialisation valeur angles —

float angleServo[nbServos]={90,90,90,90,90}; // tableau de variables de position du servo  en degrés

float angle0Servo[nbServos]={90,90,90,90,90}; // tableau de variable de la dernière position du servo  en degrés

int vitesse=10; // 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

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

//— Création objet servomoteur

Servo servo[nbServos]; // crée un tableau d’objet 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

// cf la dernière valeur utilisée avec write (ou writmicrosecond )est utilisée pour positionner les servomoteurs initialement
//— position 90 par défaut à l’attache de la broche si rien de précisé ..

//———- initialisation des servomoteurs ———
for (int i=0; i<nbServos; i++) { // passe en revue les n servomoteurs

  //—- position initiale servo i —
  servo[i].writeMicroseconds(angle(angleInitServo[i])); // crée impulsion à partir valeur angle – plus précis que write()
  //— on positionne avant d’attacher le servomoteur à la broche sinon l’angle 90° est utilisé par défaut…

  servo[i].attach(brocheServo[i]);  // attache l’objet servo à la broche de commande du servomoteur

  //— attention le positionnement initial est « brutal »

  //servoTo( servo1, angle0Servo1, angleInitServo1, vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —
  angle0Servo[i]=angleInitServo[i];

} // fin for i initialisation

// ——- Broches en sorties numériques ——-  

// ——- 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 ——-  

//——– initialisation des broches ——

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

//—————- code exemple pour présension balle entre 2 positions – version lente—-

vitesse=15; // vitesse de positionnement initial rapide

delay(1000);
servosBrasSyncIndice(90,30,45,10,45); // baisse par dessus pince ouverte

delay(1000);
servosBrasSyncIndice(90,30,45,10,85); // idem ferme pince

delay(1000);
servosBrasSyncIndice(90,120,10,10,85); // position initiale pince fermée

delay(1000);
servosBrasSyncIndice(70,20,20,130,85); // pose pince fermée

delay(1000);
servosBrasSyncIndice(70,20,20,130,45); // pose pince ouverte

delay(1000);
servosBrasSyncIndice(90,120,10,10,85); // position initiale pince fermée

//—————- code exemple pour présension balle entre 2 positions – version rapide—-
/*
vitesse=2; // vitesse de positionnement initial rapide

delay(500);
servosBrasSyncIndice(90,30,45,10,45); // baisse par dessus pince ouverte

delay(500);
servosBrasSyncIndice(90,30,45,10,85); // idem ferme pince

delay(500);
servosBrasSyncIndice(90,120,10,10,85); // position initiale pince fermée

delay(500);
servosBrasSyncIndice(70,20,20,130,85); // pose pince fermée

delay(500);
servosBrasSyncIndice(70,20,20,130,45); // pose pince ouverte

delay(500);
servosBrasSyncIndice(90,120,10,10,85); // position initiale pince fermée

*/

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

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

} // 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 vitesse(xxx)

    //————— test instruction servosBrasRSync(xxx,xxx,xxx,xxx,xxx) – position relative synchronisé

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

    //void servosRobotRSync( float S1, float S2, float S3, float S4, float S5) // reçoit les angles absolus des servomoteurs
    servosBrasRSyncIndice(float(params[0]),float(params[1]),float(params[2]),float(params[3]), float(params[4])); // positionnement synchronisé des servomoteurs
    // le nombre paramètres doit correspondre à la variable nbServos (nombre de servomoteurs)

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

  } // fin test servoBrasRSync

    //————— test instruction servosBrasSync(xxx,xxx,xxx,xxx,xxx) – position absolue synchronisé

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

    //void servosRobotRSync( float S1, float S2, float S3, float S4, float S5) // reçoit les angles absolus des servomoteurs
    servosBrasSyncIndice(float(params[0]),float(params[1]),float(params[2]),float(params[3]), float(params[4])); // positionnement synchronisé des servomoteurs
    // le nombre paramètres doit correspondre à la variable nbServos (nombre de servomoteurs)

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

  } // fin test servoBrasSync

  //================ instructions servo i =========

  //————– test instruction servo(i,xxx) ———– // positionnement brut immédiat

  if (testInstruction2(« servo(« ,2)==true) { // si instruction avec 2 paramètres reçue valide
  //format instruction (numéro servo, valeur angle absolu )
  // param[0] est le numéro du servomoteur, param[1] est la valeur de l’angle

    servo[params[0]].writeMicroseconds(angle(params[1])); // crée impulsion à partir valeur angle – plus précis que write()

    angleServo[params[0]]=params[1]; // mémorise angle actuel

  } // fin test servo1(xxx)

  //————– test instruction servoto(i,xxx) ———– // positionnement progressif absolu
  if (testInstruction2(« servoto(« ,2)==true) { // si instruction avec 2 paramètres reçue valide
  //format instruction (numéro servo, valeur angle absolu )
  // param[0] est le numéro du servomoteur, param[1] est la valeur de l’angle

    // void servoTo( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoTo( servo[params[0]], angle0Servo[params[0]], params[1], vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angle0Servo[params[0]]=params[1]; // met à jour l’angle courant servo  avec valeur extraite par testInstruction()

    Serial.print(F(« angle0Servo »)), Serial.print(params[0]),Serial.print(F( » = « )), Serial.println(angle0Servo[params[0]]);

  } // fin test servo1to(xxx)

  //————– test instruction servo1toR(i,xxx) ———– // positionnement progressif relatif
  if (testInstruction2(« servotoR(« ,2)==true) { // si instruction avec 2 paramètres reçue valide

//  if (testInstruction(« servo1toR(« )==true) { // si instruction reçue valide

    // void servoToR( Servo toServo, float fromAngle, float toAngle, int toVitesse, int toPas)
    servoToR( servo[params[0]], angle0Servo[params[0]], params[1], vitesse, 1); //— positionnement progressif par pas fixe de 1 degré —

    angle0Servo[params[0]]=angle0Servo[params[0]]+params[1]; // met à jour l’angle courant servo  avec valeur extraite par testInstruction()

    Serial.print(F(« angle0Servo »)), Serial.print(params[0]),Serial.print(F( » = « )), Serial.println(angle0Servo[params[0]]);

  } // fin test servo1toR(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 ————

//————— testInstruction2 : test si instruction de la forme instruction(xxx, 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=valeurInt48; // 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=toAnglefromAngle; // 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=fromAngle1; // 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=fromAngle1; // 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

 } // fin fonction servoToR

//————- Fonctions positionnement synchronisé avec indices servomoteurs ——————-

//———— fonction de positionnement synchronisé des servomoteurs du robot en poosition absolue ——
void servosBrasSyncIndice( float S1, float S2, float S3, float S4, float S5) { // reçoit les angles absolus des servos

  //————- tableaux de valeurs utilisés par la fonction —
  float S[nbServos]={S1,S2,S3,S4,S5}; // tableau de valeurs utilisant les paramètres reçus par la fonction

  float deltaS[nbServos]; // tableau de valeurs pour la différence entre l’angle courant et l’angle cible (deltas)
  float absDeltaS[nbServos]; // tableau de valeurs pour valeur absolues des deltas

  float cranS[nbServos];   // tableau de valeurs pour calcul des crans d’incrémentation pour chaque servomoteur

  //—- variables utilisées par la fonction
  float deltaMax=0;     //—— la plus grande valeur absolue des deltas

  //——— calcul de la variation d’angle pour chaque servomoteur

  for (int i=0; i<nbServos; i++){

    deltaS[i]=S[i]angle0Servo[i]; // — calcule la différence entre l’angle courant et l’angle cible du servo i

    absDeltaS[i]=abs(deltaS[i]); // calcul de la valeur absolue du delta du servo i  
   //— calculé ici pour éviter d’utiliser fonctions dans fonction max() – cf Référence

   //—— calcul du deltaMax = la plus grande valeur absolue des delta  
  deltaMax=max(deltaMax, absDeltaS[i]); // après tous les passages, la valeur la plus grande est conservée

  }

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

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

    for (int i=0; i<nbServos; i++){

    cranS[i]=deltaS[i]/deltaMax; // divise delta / nombre de pas à effectuer par le servomoteur i

    Serial.print(F(« cranS[« )),  Serial.print(i), Serial.print(F(« ] = « )), Serial.println(cranS[i]);

    //——– réinitialise variable angle courant des servomoteurs —-
    //- évite de modifier angle0Servo lors des calculs
    angleServo[i]=angle0Servo[i];

  } // fin for i nbServos

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

    for (int i=0; i<nbServos; i++){ // défile les n servomoteurs

        //———- servomoteur i
        angleServo[i]=angleServo[i]+cranS[i]; // ajoute cran
        servo[i].writeMicroseconds(angle(angleServo[i])); // crée impulsion à partir valeur angle – plus précis que write()
        Serial.print(F( » / S »)),Serial.print(i), Serial.print(F( » = « )), Serial.print(angleServo[i]);

    } // fin for i nbServos

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

  } // fin for j 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

    for (int i=0; i<nbServos; i++){ // défile les n servomoteurs

         angle0Servo[i]=S[i]; // nouvel angle du servomoteur i
         Serial.print(F( » / S »)),Serial.print(i), Serial.print(F(« 0 = « )), Serial.print(angle0Servo[i]);

    } // fin for i nbServos

    Serial.println();

} // fin servosRobotSyncIndice – fonction de positionnement synchronisé – angles en valeur absolue

//———— fonction de positionnement synchronisé des servomoteurs du robot en position relative ——
void servosBrasRSyncIndice(float S1, float S2, float S3, float S4, float S5) { // reçoit les angles relatifs des servos
//———— le nombre d’angle reçus par la fonction doit correspondre aux nombres de servomoteurs – constante nbServos

  //————- tableaux de valeurs utilisés par la fonction —
  float S[nbServos]={S1,S2,S3,S4,S5}; // tableau de valeurs utilisant les paramètres reçus par la fonction

  float deltaS[nbServos]; // tableau de valeurs pour la différence entre l’angle courant et l’angle cible (deltas)
  float absDeltaS[nbServos]; // tableau de valeurs pour valeur absolues des deltas

  float cranS[nbServos];   // tableau de valeurs pour calcul des crans d’incrémentation pour chaque servomoteur

  //—- variables utilisées par la fonction
  float deltaMax=0;     //—— la plus grande valeur absolue des deltas

  //——— calcul de la variation d’angle pour chaque servomoteur

  for (int i=0; i<nbServos; i++){

    deltaS[i]=S[i]; // — le delta est l’angle relatif envoyé à la fonction du servo i

    absDeltaS[i]=abs(deltaS[i]); // calcul de la valeur absolue du delta du servo i  
   //— calculé ici pour éviter d’utiliser fonctions dans fonction max() – cf Référence

   //—— calcul du deltaMax = la plus grande valeur absolue des delta  
  deltaMax=max(deltaMax, absDeltaS[i]); // après tous les passages, la valeur la plus grande est conservée

  }

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

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

    for (int i=0; i<nbServos; i++){

    cranS[i]=deltaS[i]/deltaMax; // divise delta / nombre de pas à effectuer par le servomoteur i

    Serial.print(F(« cranS[« )),  Serial.print(i), Serial.print(F(« ] = « )), Serial.println(cranS[i]);

    //——– réinitialise variable angle courant des servomoteurs —-
    //- évite de modifier angle0Servo lors des calculs
    angleServo[i]=angle0Servo[i];

  } // fin for i nbServos

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

    for (int i=0; i<nbServos; i++){ // défile les n servomoteurs

        //———- servomoteur i
        angleServo[i]=angleServo[i]+cranS[i]; // ajoute cran
        servo[i].writeMicroseconds(angle(angleServo[i])); // crée impulsion à partir valeur angle – plus précis que write()
        Serial.print(F( » / S »)),Serial.print(i), Serial.print(F( » = « )), Serial.print(angleServo[i]);

    } // fin for i nbServos

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

  } // fin for j 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

    for (int i=0; i<nbServos; i++){ // défile les n servomoteurs

         angle0Servo[i]=angle0Servo[i]+S[i]; // S[i]
         Serial.print(F( » / S »)),Serial.print(i), Serial.print(F(« 0 = « )), Serial.print(angle0Servo[i]);

    } // fin for i nbServos

    Serial.println();

} // fin servosRobotRSyncIndice – fonction de positionnement synchronisé

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

void reset(void) {

  servosBrasSyncIndice(angleInitServo[0],angleInitServo[1],angleInitServo[2],angleInitServo[3],angleInitServo[4]); // repositionne avec les angles initiaux

} // fin reset

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

 

Noter cet article

Laisser un commentaire

Votre adresse e-mail ne sera pas publiée. Les champs obligatoires sont indiqués avec *

Archive Mon Club Elec

Articles populaires

Newsletter

Inscrivez-vous maintenant et bénéficiez d'un soutien continu pour réaliser vos travaux électriques en toute sécurité.