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.
L’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 Policydoit 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 :
- La fonction
setAllBaudRate (const hkxBaudrate &baudrate)modifie la vitesse de communication des servomoteurs. Nous l’avons vu plus haut, cette vitesse de communication ne peut prendre que 7 valeurs (dont une fortement déconseillée). Nous optons pour la valeur la plus rapide soit666666bps. Il s’agit du seul paramètre pour lequel il n’existe pas de fonction avec le préfixe “setOne” car appliquer ce changement à un seul servomoteur n’aurait pas de sens. - La fonction
setAllLoadControl(uint16_t newDeadZone, uint8_t newSaturatorOffset, const uint32_t &newSaturatorSlope, int8_t newPWMOffset, uint8_t newMinPWM, uint16_t newMaxPWM)modifie le profil de la commande de charge. L’influence des paramètres est synthétisée par le schéma ci-dessous.
Les valeurs proposées donne la courbe – obtenue avec une petite routine scilab :
- La fonction
setAllPWMPositionLimits(uint16_t newOverlaodPWMThreshold, int16_t newMinPosition, int16_t newMaxPosition)modifie les limites de charge et de position. Si ces limites sont dépassées, elles déclenchent un mode erreur des servomoteurs. - La fonction
setAllControlSystem(uint16_t kProportionnal, uint16_t kDerivative, uint16_t kInteger, uint16_t feedforwardGain1, uint16_t feedforwardGain2)modifie l’asservissement (PID). Les valeurs proposées donne un asservissement plus ferme que celui par défaut.
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).
- La fonction
setAllInPositionCriteria (uint16_t stopThreshold, uint16_t inPositionMargin)modifie les critères de positionnement. Les valeurs proposées donne une meilleure précision (0,3°) que celles par défaut (0,9°). - La fonction
scanIDs(boolean IDs[])balaye tous les identifiants pour vérifier s’ils sont connectés.IDsest un tableau de 254 booléens, siIDs[ID]est vrai alorsIDest connecté, sinon il ne l’est pas. - La fonction
getBehaviour(uint8_t ID, HkxMaybe<uint16_t> voltage, HkxMaybe<uint16_t> temperature, HkxMaybe<hkxControlMode> controlMode, HkxMaybe<uint16_t> tick, HkxMaybe<int16_t> absolutePosition, HkxMaybe<int16_t> velocity, HkxMaybe<uint16_t> PWM, HkxMaybe<int16_t> absoluteGoalPosition, HkxMaybe<int16_t> absoluteDesiredTrajectoryPosition, HkxMaybe<int16_t> desiredVelocity)retourne le comportement instantané. Dans notre exemple, seules la tension et la température sont retournées. Toutes les valeurs marquéesHKX_NO_VALUEne sont pas retournées. - La fonction
getStatus(uint8_t ID, HkxStatus &statusED)retourne le statut. Le statut renseigne si le servomoteur est dans un mode erreur (en général cela fait clignoter sa DEL en rouge). Une classe spécifique au statut permet de faciliter son utilisation, en particulier pour identifier l’origine de l’erreur. Par exemple, sa fonctionisError(uint8_t mask)renseigne si l’erreur mentionnée par le masque est présente. Consultez le tableau du manuel des servomoteurs page 39 pour plus de détails sur le statut, et la liste des masques disponibles dans le manuel de la librairie. - La fonction
clearAllStatus()permet de sortir tous les servomoteurs du mode erreur. Notez que pour certaines erreurs (tensions d’alimentation trop faible par exemple), le mode erreur se réenclenche instantanément et cela ne règle pas le problème.
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 :
- La fonction
getStatus(HkxStatus &statusED, boolean update)retourne le statut. Elle fonctionne sur le même principe que lors de la configuration, à la seule différence qu’ici, l’instanceservostocke le statut à chaque paquet reçu du servomoteur. Siupdateest faux, le statut renvoyé est le dernier reçu, s’il est vrai, une requête de demande de statut est envoyée. La fonctionclearStatus()fonctionne aussi comme pour la configuration. - La fonction
setTorqueLEDControl(HkxMaybe<hkxTorqueControl> newTorqueControl, HkxMaybe<hkxLEDControl> newLEDControl)permet d’activer la commande de charge, via le paramètrenewTorqueControlqui peut prendre 3 valeurs :HKX_TORQUE_FREE(pas de résistance au mouvement, ou plutôt seulement celle due aux engrenages),HKX_TORQUE_BREAK(le servomoteur resiste au mouvement, mais ne cherche pas à maintenir sa position) etHKX_TORQUE_ON(le moteur cherche à maintenir sa position en appliquant la commande de charge) ; elle permet aussi d’activer la DEL qui peut prendre les valeurs suivante :HKX_LED_OFF,HKX_LED_GREEN,HKX_LED_BLUE,HKX_LED_RED,HKX_LED_CYAN,HKX_LED_YELLOW,HKX_LED_PINK, etHKX_LED_WHITE. Pour ces deux paramètres, la valeurHKX_NO_VALUEconserve leur valeur courrante. - La fonction
movePosition(int16_t destinationAngle, uint16_t playTime, HkxMaybe<hkxLEDControl> LEDControl, bool waitStop)permet d’effectuer un déplacement vers la positiondestinationAngleen une duréeplayTime, avec la DEL de couleurLEDControl. Le paramètrewaitStoprenseigne si on souhaite attendre la fin du mouvement pour exécuter la suite du programme. S’il est vrai, alors, à la fin de l’exécution la DEL et la commande de charge reviennent leur état d’avant le mouvement. Nous voyons ces deux cas dans notre code. - La fonction
getBehaviour(HkxMaybe<uint16_t> inputVoltage, HkxMaybe<uint16_t> temperature, HkxMaybe<int16_t> position, HkxMaybe<int16_t> velocity, HkxMaybe<uint16_t> PWM, HkxMaybe<int16_t> goalPosition, HkxMaybe<int16_t> trajectoryPosition, HkxMaybe<int16_t> trajectoryVelocity)fonctionne de manière identique à la configuration. - La fonction
isInPosition()retourne si le servomoteur est arrivé à sa destination, c’est à dire si le mouvement est fini. Cette fonction est intéressante pour pouvoir exécuter du code pendant le mouvement et vérifier si ce dernier est terminé. Elle présente aussi l’intérêt de permettre de détecter qu’un déplacement n’est pas arrivé à destination (à cause d’un obstacle par exemple).
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 :
- La fonction
setAllTorqueLEDControl(HkxMaybe<hkxTorqueControl> newTorqueControl, HkxMaybe<hkxLEDControl> newLEDControl)permet d’activer la commande de charge et la DEL. Elle fonctionne identiquement à celle de la classe HkxPosControl, mais s’applique à tous les servomoteurs du groupe. - La fonction
moveSyncAllPosition(int16_t destinationAngle[], uint16_t playTime, HkxMaybe<hkxLEDControl> LEDControl[], bool waitStop)permet d’effectuer un déplacement de la même façon que pour la classe HkxPosControl. Ce déplacement étant synchronisé, une seule durée (playTime) est nécessaire, les autres entrées sont des tableaux contenant autant de valeurs que de servomoteurs. - La fonction
moveAsyncAllPosition (int16_t destinationAngle[], uint16_t playTime[], HkxMaybe<hkxLEDControl> LEDControl[], bool waitStop)permet d’effectuer un déplacement de la même façon que ci-dessus. Ce déplacement étant asynchrone, la durée (playTime) aussi doit être définie pour chaque servomoteur.
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.