HerkuleXLib: exemple d’utilisation

Dans cet article la librairie HerkuleXLib est utilisée dans sa version 1.1 et Arduino IDE dans sa version 1.6.5.

Cet article donne des exemples d’utilisation de servomoteurs HerkuleX DRS-0101 avec une carte Arduino MEGA. Pour plus de détails sur les fonctions de la librairie, reportez-vous à la documentation utilisateur (téléchargeable et consultable en ligne). Pour plus de détails sur le fonctionnement des servomoteurs, reportez-vous au manuel utilisateur (téléchargeable).

1 Préparation

Les 3 servomoteurs sont branchés sur le port Serial1 de la carte Arduino. Cette dernière est aussi branchée à l’ordinateur via le port USB pour retourner les informations au travers du terminal série. La librairie HerkuleXLib doit être installée comme décrit dans son manuel opérateur.

HerkuleXLib: wiring 3 servos

HerkuleXLib: cablage photoL’architecture général de la librairie est décrite dans un article précédent.

2 Configurer des servomoteurs

2.1 Introduction

Les servomoteurs possèdent 60 paramètres qu’il est possible de consulter et/ou modifier. Ils sont listés pages 26 à 29 du manuel opérateur des servomoteurs. La librairie HerkuleXLib permet de les consulter/modifier sauf :

  • Les paramètres Reserved
  • ACK Policy doit avoir la valeur 1 (valeur par défaut), sans quoi la librairie ne fonctionne pas correctement.

Les fonctions disponibles pour la configuration sont celles de la classe HkxSetup. Les paramètres ont été regroupés par fonctionnalités, précédées de “get” pour lire la valeur ou de “set” pour modifier les valeurs. Le préfixe “setOne” ne s’applique qu’à un servomoteur, quand “setAll” permet de modifier les valeurs de paramètres pour tous les servomoteurs connectés.

2.2 Créer le programme

L’écriture du code se fait avec le logiciel Arduino IDE. Lancez le logiciel, créez un nouveau fichier (menu Fichier > Nouveau), et sauvegardez-le (menu Fichier > Sauvegarder) – nommez-le comme il vous convient. Inclure HkxSetup en en-tête du projet. Créons maintenant les instances des classes HkxPrint, HkxCommunication et  HkxPosControl.

#include <HkxSetup.h>

void setup() {
  delay(5000); // wait 5 seconds to open the serial monitor
  HkxPrint printoutLib = HkxPrint(Serial, 9600);  // Printout errors from the library on the serial monitor
  HkxPrint printoutCode = HkxPrint(printoutLib, true, true, true);  // Printout from the code on the serial monitor
  HkxCommunication communication = HkxCommunication(HKX_115200, Serial1, printoutLib);  // Communication with the servo on Serial1
  HkxSetup setupServos(communication, printoutLib);  // setup the servos
  
}

void loop() {
  // put your main code here, to run repeatedly:

}

La classe HkxPrint gère le retour des données (messages d’information, d’alerte et d’erreur) au travers d’un port série. L’instance printoutLib retourne les messages par défaut (les erreurs seulement) sur le port Serial (USB) à 9600 bps. L’instance printoutCode est une copie de printoutLib qui retourne par contre tous les types de messages (informations, alertes et erreurs).

La classe HkxCommunication gère la communication avec les servomoteurs, elle s’assure que les données envoyées et reçues respectent le protocole (manuel des servomoteurs pages 18 à 20 et 40 à 50). Sa première variable est la vitesse de transmission qui peut prendre les valeurs HKX_57600, HKX_115200, HKX_200000, HKX_250000, HKX_400000, HKX_500000, et HKX_666666, correspondant respectivement à des vitesses de 57600, 115200, 200000, 250000, 400000, 500000 et 666666 bps. L’instance communication travaille sur le port Serial1, à 115200 bps (valeur par défaut), les messages étant retournés par l’instance printoutLib.

Comme mentionné dans le manuel de la librairie, la vitesse de transmission à 57600 bps est fortement déconseillée pour les servomoteurs car elle produit des erreurs de communication.

La classe HkxSetup permet de configurer les servomoteurs. L’instance setupServos communique avec les servomoteurs au travers de l’instance communication et retournera ses messages par printoutLib.

2.3 J’ai un problème d’identité

Le protocole des servomoteurs attache une instruction à un identifiant (ID), or par défaut, les servomoteurs ont le même identifiant ID = 253 ! Nous commençons par donner une valeur spécifique à chaque servomoteur ID = 0, ID = 1 et ID = 2. Pour cela il faut brancher un servomoteur à la fois, puis exécuter le code précédemment écrit en y rajoutant les lignes suivantes, et en modifiant la valeur de newID pour chacun d’entre eux. Le processus s’est bien passé si le message Info: ID modified :) s’affiche sur le terminal série.

  // variables
  byte newID = 2; // change the value for each servo
  HkxStatus statusED;
  byte err = 0;

  // check the ID=253 is connected
  err = setupServos.getStatus (253, statusED);
  if(err != 0){ 
    printoutCode.errorPrint("The servo is not connected");
    return;
  }

  // change the ID
  setupServos.setID(253, newID);

  // check the change has been correctly performed
  err = setupServos.getStatus (newID, statusED);
  if(err != 0){ 
    printoutCode.errorPrint("ID modification did not performed correctly");
    return;
  }
  printoutCode.infoPrint("ID modified :)");

Les variables statusED et err, et la fonction getStatus(uint8_t ID, HkxStatus &statusED) nous permettent de vérifier qu’un ID est bien connecté. La fonction setID (uint8_t ID, uint8_t newID) modifie l’ID du servomoteur.

Vous pouvez télécharger le code ici.

Maintenant que vous avez réglé votre problème d’identité, vous pouvez rebrancher tous les servomoteurs pour passer à la suite.

2.4. Configuration des servomoteurs

Passons à l’écriture du code pour configurer les servomoteurs en repartant du code du paragraphe 2.2. Nous accélérerons la vitesse de communication, puis modifions le profil de la commande de charge pour rajouter un effet élastique, réduisons la limite admissible de la charge, et  modifions l’asservissement (PID) ainsi que les critères de position. Nous partons enfin à la recherche de nos identités, de notre comportement et de notre statut (vaste programme !).

void setup() {
  delay(5000); // wait 5 seconds to open the serial monitor
  HkxPrint printoutLib = HkxPrint(Serial, 9600);  // Printout errors on the serial monitor
  HkxPrint printoutCode = HkxPrint(printoutLib, true, true, true);  // Printout errors on the serial monitor
  HkxCommunication communication = HkxCommunication(HKX_115200, Serial1, printoutLib);  // Communication with the servo on Serial1
  HkxSetup setupServos(communication, printoutLib);  // setup the servos

  // Variables
  HkxStatus statusED;
  bool IDs[254];
  uint16_t voltage;
  uint16_t temperature;
  String message;

  // Modify the baudrate
  setupServos.setAllBaudRate(HKX_666666);

  // Modify the Load controls
  setupServos.setAllLoadControl(0, 200, 50, 0, 0, 550);

  // Modify the PWM and position limits
  setupServos.setAllPWMPositionLimits(600, -1667, 1665);

  // Modify the control system
  setupServos.setAllControlSystem(500, 10000, 0, 20000, 0);

  // Modify the position criteria
  setupServos.setAllInPositionCriteria(3, 3);

  // Scan the IDs 
  printoutCode.infoPrint("Start the ID scan (can take some time)");
  setupServos.scanIDs(IDs);
  printoutCode.infoPrint("ID scan performed");

  for(int ID=0 ; ID < 254 ; ID++){
    if(IDs[ID]){
      message = "ID = ";
      message += ID;
      printoutCode.infoPrint(message);
      
      // Get the behaviour
      setupServos.getBehaviour(ID, &voltage, &temperature, HKX_NO_VALUE, HKX_NO_VALUE, HKX_NO_VALUE, HKX_NO_VALUE, HKX_NO_VALUE, HKX_NO_VALUE, HKX_NO_VALUE, HKX_NO_VALUE);
      message = "voltage = ";
      message += (float)voltage/1000;
      message += " V \t temperature = ";
      message += (float)temperature/100;
      message += " °C";
      printoutCode.infoPrint(message);
      
      // Get the status
      setupServos.getStatus(ID, statusED);
      if(statusED.isError(HKX_STAT_ALL)){
        printoutCode.errorPrint("The servo has an error");
        if(statusED.isError(HKX_STAT_VOLTAGE)){
          printoutCode.errorPrint("Exceed Input Voltage limit");
        } if(statusED.isError(HKX_STAT_POSITION)){
          printoutCode.errorPrint("Exceed allowed POT limit");
        } if(statusED.isError(HKX_STAT_TEMPERATURE)){
          printoutCode.errorPrint("Exceed Temperature limit");
        } if(statusED.isError(HKX_STAT_PACKET)){
          printoutCode.errorPrint("Invalid Packet");
        } if(statusED.isError(HKX_STAT_OVERLOAD)){
          printoutCode.errorPrint("Overload detected");
        } if(statusED.isError(HKX_STAT_DRIVER)){
          printoutCode.errorPrint("Driver fault detected");
        } if(statusED.isError(HKX_STAT_ROM_DISTORTED)){
          printoutCode.errorPrint("EEP REG distorted");
        }
        setupServos.getStatus(ID, statusED);
      }
    }
  }
  setupServos.clearAllStatus ();
}

Je vais décortiquer avec vous ce morceau de code :

HerkuleXLib: Commande chargeLes valeurs proposées donne la courbe – obtenue avec une petite routine scilab :

HerkuleXLib: Figure load control curve

La modification de l’asservissement, en particulier lorsqu’on le rend plus ferme, peut aboutir à des cas d’utilisation hors des limites du servomoteur et entraîner de la casse. C’est la raison pour laquelle nous abaissons aussi la charge maximale (profil de la commande de charge) et la charge limite (qui déclenche une erreur).

Pour les fonctions que nous venons d’énumérer, prenez le temps de vérifier les unités et les grandeurs dans manuel de la librairie pour les méthodes de la classe HkxSetup et pour les méthodes de la classe HkxStatus.

Le terminal série devrait afficher quelque chose ressemblant à

Info:    Start the ID scan (can take some time)
Info:    ID scan performed
Info:    ID = 0
Info:    voltage = 8.21 V 	 temperature = 26.02 °C
Error:   The servo has an error
Error:   Invalid Packet
Info:    ID = 1
Info:    voltage = 8.21 V 	 temperature = 28.50 °C
Error:   The servo has an error
Error:   Invalid Packet
Info:    ID = 2
Info:    voltage = 8.29 V 	 temperature = 27.25 °C
Error:   The servo has an error
Error:   Invalid Packet

Notez que l’erreur Invalid Packet est due au fait que j’ai exécuté le code plusieurs fois pour les tests et donc la première requête est envoyée avec une mauvaise vitesse de communication, ce qui génère des paquets erronés. Si vous exécutez une deuxième fois le code, vous aurez la même erreur. Aussi, vous pouvez vérifier que vous obtenez en plus une erreur de tension en débranchant l’alimentation 7,4 V pour ne conserver que l’alimentation de l’USB (5 V). La vidéo suivante montre l’exécution du code.

Vous pouvez télécharger le code ici.

3 Piloter 1 servomoteur

3.1 Créer le programme

Créons un nouveau fichier (menu Fichier > Nouveau), et sauvegardons-le (menu Fichier > Sauvegarder) – nommez-le comme il vous convient. Inclure HkxPosControl en en-tête du projet. Créons maintenant les instances des classes HkxPrint, HkxCommunication et  HkxPosControl.

#include <HkxPosControl.h>

void setup() {
  // INPUT
  ID = 0; // servo to drive
  
  delay(5000); // wait 5 seconds to open the serial monitor
  HkxPrint printoutLib = HkxPrint(Serial, 9600);  // Printout errors on the serial monitor
  HkxPrint printoutCode = HkxPrint(printoutLib, true, true, true);  // Printout errors on the serial monitor
  HkxCommunication communication = HkxCommunication(HKX_666666, Serial1, printoutLib);  // Communication with the servo on Serial1
  HkxPosControl servo(ID, communication, printoutLib);  // control position for the servo ID

}

void loop() {
  // put your main code here, to run repeatedly:

}

Nous voyons que le début du code est très semblable à celui de la configuration. Seulement, au lieu de créer une instance de HkxSetup, qui permet de configurer tous les servomoteurs branchés, quelque soit leur nombre, nous créons une instance de HkxPosControl qui est dédiée à piloter 1 servomoteur défini. L’instance servo communique avec les servomoteurs au travers de l’instance communication et retournera ses messages par printoutLib.

3.2 En route pilote

Nous pouvons maintenant piloter notre servomoteur. Nous nous repenchons sur le statut en injectant une trame erronée. Activons ensuite la DEL en rouge, puis faisons un mouvement avec attente de l’arrivée, et enfin un second mouvement pour lequel nous reprenons la main tout de suite pour visualiser le comportement du servomoteur en temps réel pendant le mouvement.

void setup() {
  // INPUT
  byte ID = 0;
  
  delay(5000); // wait 5 seconds to open the serial monitor
  HkxPrint printoutLib = HkxPrint(Serial, 9600);  // Printout errors on the serial monitor
  HkxPrint printoutCode = HkxPrint(printoutLib, true, true, true);  // Printout errors on the serial monitor
  HkxCommunication communication = HkxCommunication(HKX_666666, Serial1, printoutLib);  // Communication with the servo on Serial1
  HkxPosControl servo(ID, communication, printoutLib);  // control position for the servo ID

  // Variables
  HkxStatus statusED;
  String message;
  uint16_t voltage;
  uint16_t temperature;
  int16_t position;
  int16_t velocity;
  uint16_t PWM;
  int16_t goalPosition;
  int16_t trajectoryPosition;
  int16_t trajectoryVelocity;

  // Generate a packet error
  byte errorPacket[] = {0xFF, 0xFF, 0x00, 0x22, 0x43, 0xA1, 0x08};
  Serial1.write(errorPacket, 7);
  delay(1000);
  // Check if there is any statu error
  servo.getStatus(statusED, true);
  if(statusED.isError(HKX_STAT_ALL)){
    printoutCode.errorPrint("The servo has an error");
    if(statusED.isError(HKX_STAT_VOLTAGE)){
      printoutCode.errorPrint("Exceed Input Voltage limit");
    } if(statusED.isError(HKX_STAT_POSITION)){
      printoutCode.errorPrint("Exceed allowed POT limit");
    } if(statusED.isError(HKX_STAT_TEMPERATURE)){
      printoutCode.errorPrint("Exceed Temperature limit");
    } if(statusED.isError(HKX_STAT_PACKET)){
      printoutCode.errorPrint("Invalid Packet");
    } if(statusED.isError(HKX_STAT_OVERLOAD)){
      printoutCode.errorPrint("Overload detected");
    } if(statusED.isError(HKX_STAT_DRIVER)){
      printoutCode.errorPrint("Driver fault detected");
    } if(statusED.isError(HKX_STAT_ROM_DISTORTED)){
      printoutCode.errorPrint("EEP REG distorted");
    }
  }
  servo.clearStatus();

  // Set the LED and torque off
  servo.setTorqueLEDControl(HKX_TORQUE_FREE, HKX_LED_OFF);  
  printoutCode.infoPrint("LED is off, torque is free");
  delay(5000); // wait 5 seconds

  // Set the LED to red
  servo.setTorqueLEDControl(HKX_NO_VALUE, HKX_LED_RED);  
  printoutCode.infoPrint("LED is now red");
  delay(5000); // wait 5 seconds

  // Set the servo to position 45° in 2 seconds, led turns to blue during the move, 
  // wait for the end of the move
  printoutCode.infoPrint("Start moving to 45 degrees in 2 seconds, blue LED");
  servo.movePosition(450, 2000, HKX_LED_BLUE, true);  
  printoutCode.infoPrint("The move is finished, torque is free and LED back to red");

  // Get the current behaviour of the servo
  printoutCode.infoPrint("Volt \t Temp \t Pos \t Vel \t PWM \t gPos \t tPos \t tVel");
  servo.getBehaviour(&voltage, &temperature, &position, &velocity, &PWM, &goalPosition, &trajectoryPosition, &trajectoryVelocity);
  message = String((float)voltage/1000);
  message += " \t ";
  message += (float)temperature/100;
  message += " \t ";
  message += (float)position/10;
  message += " \t ";
  message += velocity;
  message += " \t ";
  message += PWM;
  message += " \t ";
  message += (float)goalPosition/10;
  message += " \t ";
  message += (float)trajectoryPosition/10;
  message += " \t ";
  message += trajectoryVelocity;
  printoutCode.infoPrint(message);
  delay(5000); // wait 5 seconds

  // Set the servo to position -45° in 2 seconds, led turns to green, 
  // do NOT wait for the end of the move
  printoutCode.infoPrint("Start moving to -45 degrees in 2 seconds, green LED");
  printoutCode.infoPrint("Volt \t Temp \t Pos \t Vel \t PWM \t gPos \t tPos \t tVel");
  servo.movePosition(-450, 2000, HKX_LED_GREEN, false);  
  // Get the current behaviour of the servo during the move
  while(!servo.isInPosition()){
    servo.getBehaviour(&voltage, &temperature, &position, &velocity, &PWM, &goalPosition, &trajectoryPosition, &trajectoryVelocity);
    message = String((float)voltage/1000);
    message += " \t ";
    message += (float)temperature/100;
    message += " \t ";
    message += (float)position/10;
    message += " \t ";
    message += velocity;
    message += " \t ";
    message += PWM;
    message += " \t ";
    message += (float)goalPosition/10;
    message += " \t ";
    message += (float)trajectoryPosition/10;
    message += " \t ";
    message += trajectoryVelocity;
    printoutCode.infoPrint(message);
  }
  printoutCode.infoPrint("The move is finished, torque is keep on and LED stays green");
}

Regardons de plus près les fonctions utilisées :

Pour les fonctions que nous venons d’énumérer, prenez le temps de vérifier les unités et les grandeurs dans manuel de la librairie pour les méthodes de la classe HkxPosControl et pour les méthodes de la classe HkxStatus.

Le terminal série nous donne

Error:   The servo has an error
Error:   Invalid Packet
Info:    LED is off, torque is free
Info:    LED is now red
Info:    Start moving to 45 degrees in 2 seconds, blue LED
Info:    The move is finished, torque is free and LED back to red
Info:    Volt 	 Temp 	 Pos 	 Vel 	 PWM 	 gPos 	 tPos 	 tVel
Info:    8.14 	 24.39 	 45.30 	 0 	 0 	 45.30 	 45.30 	 0
Info:    Start moving to -45 degrees in 2 seconds, green LED
Info:    Volt 	 Temp 	 Pos 	 Vel 	 PWM 	 gPos 	 tPos 	 tVel
Info:    8.14 	 24.39 	 45.30 	 0 	 0 	 -45.30 	 45.30 	 0
Info:    8.14 	 24.39 	 44.90 	 0 	 0 	 -45.30 	 44.90 	 -1076
Info:    8.14 	 24.39 	 44.60 	 0 	 961 	 -45.30 	 44.30 	 -2239
Info:    8.07 	 24.39 	 43.30 	 0 	 899 	 -45.30 	 42.70 	 -3432
Info:    8.14 	 24.39 	 41.70 	 -29 	 899 	 -45.30 	 41.00 	 -4625
Info:    8.07 	 24.39 	 39.10 	 -58 	 200 	 -45.30 	 38.40 	 -5876
Info:    8.07 	 24.39 	 36.10 	 -58 	 836 	 -45.30 	 35.20 	 -7127
Info:    8.07 	 24.39 	 32.60 	 -29 	 200 	 -45.30 	 31.60 	 -8377
Info:    8.07 	 24.39 	 29.00 	 -58 	 824 	 -45.30 	 27.30 	 -8785
Info:    8.07 	 24.39 	 25.40 	 -58 	 201 	 -45.30 	 23.10 	 -8785
Info:    8.07 	 24.39 	 21.80 	 -29 	 201 	 -45.30 	 18.90 	 -8785
Info:    8.14 	 24.39 	 18.20 	 -58 	 201 	 -45.30 	 14.60 	 -8785
Info:    8.07 	 24.39 	 14.90 	 -29 	 822 	 -45.30 	 10.40 	 -8785
Info:    8.14 	 24.39 	 11.40 	 -58 	 822 	 -45.30 	 6.10 	 -8785
Info:    8.14 	 24.39 	 7.40 	 -58 	 187 	 -45.30 	 1.90 	 -8785
Info:    8.14 	 24.39 	 4.20 	 -58 	 821 	 -45.30 	 -2.20 	 -8785
Info:    8.14 	 24.39 	 0.30 	 -58 	 821 	 -45.30 	 -6.50 	 -8785
Info:    8.14 	 24.39 	 -3.90 	 -58 	 961 	 -45.30 	 -10.70 	 -8785
Info:    8.14 	 24.39 	 -7.80 	 -58 	 821 	 -45.30 	 -14.90 	 -8785
Info:    8.14 	 24.39 	 -12.00 	 -58 	 821 	 -45.30 	 -19.20 	 -8785
Info:    8.07 	 24.39 	 -16.30 	 -58 	 821 	 -45.30 	 -23.70 	 -8785
Info:    8.14 	 24.39 	 -20.80 	 -87 	 821 	 -45.30 	 -28.00 	 -8785
Info:    8.14 	 24.39 	 -25.40 	 -58 	 821 	 -45.30 	 -32.20 	 -8145
Info:    8.07 	 24.39 	 -29.60 	 -58 	 821 	 -45.30 	 -36.10 	 -6865
Info:    8.14 	 24.39 	 -33.90 	 -58 	 822 	 -45.30 	 -39.10 	 -5585
Info:    8.07 	 24.39 	 -37.10 	 -58 	 822 	 -45.30 	 -41.70 	 -4305
Info:    8.14 	 24.39 	 -40.40 	 -29 	 823 	 -45.30 	 -43.30 	 -3025
Info:    8.07 	 24.39 	 -43.30 	 -58 	 824 	 -45.30 	 -44.60 	 -1745
Info:    8.14 	 24.39 	 -44.90 	 0 	 961 	 -45.30 	 -45.30 	 -465
Info:    8.14 	 24.39 	 -45.30 	 0 	 0 	 -45.30 	 -45.30 	 0
Info:    8.14 	 24.39 	 -45.30 	 0 	 0 	 -45.30 	 -45.30 	 0
Info:    8.14 	 24.39 	 -45.30 	 0 	 0 	 -45.30 	 -45.30 	 0
Info:    8.14 	 24.39 	 -45.30 	 0 	 0 	 -45.30 	 -45.30 	 0
Info:    8.14 	 24.39 	 -45.30 	 0 	 0 	 -45.30 	 -45.30 	 0
Info:    The move is finished, torque is keep on and LED stays green

La vidéo suivante montre l’exécution du code.

Vous pouvez télécharger le code ici.

4 Piloter 3 servomoteurs

4.1 Créer le programme

Repartons d’une base saine : créons un nouveau fichier (menu Fichier > Nouveau), et sauvegardons-le (menu Fichier > Sauvegarder). Inclure cette fois HkxGroupPosControl en en-tête du projet. Créons enfin les instances des classes HkxPrint, HkxCommunication et  HkxGroupPosControl.

#include <HkxGroupPosControl.h>

void setup() {
  // INPUT
  const byte nbServos = 3;
  byte IDs[nbServos] = {0, 1, 2};

  delay(5000); // wait 5 seconds to open the serial monitor
  HkxPrint printoutLib = HkxPrint(Serial, 9600);  // Printout errors on the serial monitor
  HkxPrint printoutCode = HkxPrint(printoutLib, true, true, true);  // Printout errors on the serial monitor
  HkxCommunication communication = HkxCommunication(HKX_666666, Serial1, printoutLib);  // Communication with the servo on Serial1
  HkxPosControl servo0 = HkxPosControl(IDs[0], communication, printoutLib);
  HkxPosControl servo1 = HkxPosControl(IDs[1], communication, printoutLib);
  HkxPosControl servo2 = HkxPosControl(IDs[2], communication, printoutLib);
  HkxPosControl* arrayServos[nbServos] = {&servo0, &servo1, &servo2};
  HkxGroupPosControl HerkGroupServo = HkxGroupPosControl(nbServos, arrayServos, printoutCode);

}

void loop() {
  // put your main code here, to run repeatedly:

}

Vous voyez que le code est identique au précédent à la différence près que nous créons autant d’instances de HkxPosControl qu’il y a de servomoteurs. Ces instances sont fournies à l’instance de HkxGroupPosControl sous forme d’un tableau de pointeurs pour éviter les copies inutiles. Je vous invite une fois de plus à consulter les détails du constructeur dans le manuel de la librairie.

4.2 Piloter 3 servomoteurs : pour les as du volant !

Nous pouvons maintenant piloter nos servomoteurs. Nous commençons par activer la DEL en mauve et mettre la commande de charge en frein. Nous faisons ensuite un premier mouvement synchronisé (même durée de mouvement) des 3 servomoteurs, et enfin un second mouvement asynchrone (durée de mouvement différente).

void setup() {
  // INPUT
  const byte nbServos = 3;
  byte IDs[nbServos] = {0, 1, 2};

  delay(5000); // wait 5 seconds to open the serial monitor
  HkxPrint printoutLib = HkxPrint(Serial, 9600);  // Printout errors on the serial monitor
  HkxPrint printoutCode = HkxPrint(printoutLib, true, true, true);  // Printout errors on the serial monitor
  HkxCommunication communication = HkxCommunication(HKX_666666, Serial1, printoutLib);  // Communication with the servo on Serial1
  HkxPosControl servo0 = HkxPosControl(IDs[0], communication, printoutLib);
  HkxPosControl servo1 = HkxPosControl(IDs[1], communication, printoutLib);
  HkxPosControl servo2 = HkxPosControl(IDs[2], communication, printoutLib);
  HkxPosControl* arrayServos[nbServos] = {&servo0, &servo1, &servo2};
  HkxGroupPosControl HerkGroupServo = HkxGroupPosControl(nbServos, arrayServos, printoutLib);

  // Set the torque to free and the LED off
  HerkGroupServo.setAllTorqueLEDControl(HKX_TORQUE_FREE, HKX_LED_OFF);
  delay(5000); // wait 5 seconds

  // Set the torque to break and the LED to yellow
  HerkGroupServo.setAllTorqueLEDControl(HKX_TORQUE_BREAK, HKX_LED_YELLOW);
  printoutCode.infoPrint("Torque is break, LED is yellow");
  delay(5000); // wait 5 seconds

  // Move servos to positions {45°, 30°, 15°} in 2 seconds, leds turned to pink,
  // wait for the end of the move
  printoutCode.infoPrint("Start moving to 45°, 30° and 15° in 2 seconds, pink LED");
  int16_t tabPos[] = {450, 300, 150};
  HkxMaybe<hkxLEDControl> tabLed[] = {HKX_LED_PINK, HKX_LED_PINK, HKX_LED_PINK};
  HerkGroupServo.moveSyncAllPosition(tabPos, 2000, tabLed, true); 
  printoutCode.infoPrint("The move is finished, torque is break and LED back to yellow");
  delay(5000); // wait 5 seconds

  // Move servos to positions {-45°, -30°, -15°} in 2, 1, 0.5 seconds, leds turned to pink,
  // wait for the end of the move
  printoutCode.infoPrint("Start moving to -45°, -30° and -15° in 2, 1, .5 seconds, pink LED");
  int16_t tabPos2[] = {-450, -300, -150};
  uint16_t tabTime[] = {2000, 1000, 500};
  HerkGroupServo.moveAsyncAllPosition(tabPos2, tabTime, tabLed, true); 
  printoutCode.infoPrint("The move is finished, torque is break and LED back to yellow");
}

Regardons de plus près ce code :

Pour les fonctions que nous venons d’énumérer, prenez le temps de vérifier les unités et les grandeurs dans manuel de la librairie.

Le terminal affiche

Info:    Torque is break, LED is yellow
Info:    Start moving to 45°, 30° and 15° in 2 seconds, pink LED
Info:    The move is finished, torque is break and LED back to yellow
Info:    Start moving to -45°, -30° and -15° in 2, 1, .5 seconds, pink LED
Info:    The move is finished, torque is break and LED back to yellow

La vidéo suivante montre l’exécution du code.

Vous pouvez télécharger le code ici.

Laisser un commentaire

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