Tutoriel Arduino : asservissement en vitesse d’un moteur à courant continu

Introduction

L’asservissement en vitesse d’un moteur à courant continu est la plupart du temps nécessaire pour les robots mobiles. On peut éventuellement se satisfaire d’un servomoteur à rotation continu dans le cas de petits robots mais dans un cas plus général (comme pour le robot gyropode Geeros), il sera préférable d’utiliser un moteur à courant continu avec réducteur, associé à un codeur incrémental (pour mesurer la vitesse de rotation). Le calcul de l’asservissement sera réalisé par un Arduino. Nous allons détailler tout ceci dans la suite de cet article.

Matériel utilisé

Ce tutoriel peut être mis en application facilement avec l’expérience « Commande de moteur électrique ».

La carte Romeo est intéressante car elle intègre de façon très compacte le micro-contrôleur (AVR Atmega328, le même cœur qu’un Arduino Uno) et un double pont en H permettant de contrôler deux moteurs à courant continu avec un courant max de 2 A en permanence (jusqu’à 3 A en pic). Ce composant est indispensable pour fournir suffisamment d’énergie au moteur et lui permettre de fonctionner à vitesse variable et dans les deux sens de rotation.

Si vous utilisez d’autres éléments pour ce tutoriel, faites un choix cohérent :

  • La tension de la batterie doit correspondre à celle du moteur (pour ne pas griller ce dernier avec une fausse manipulation)
  • Le courant max du moteur doit correspondre à ce que peut supporter le driver de puissance

Concernant ce dernier point, dans la pratique les risques sont très limités si vous ne bloquez pas le moteur. Le courant à vide est en général relativement faible ; le courant max est observé uniquement lorsque le moteur est bloqué alors qu’il est alimenté avec sa tension maximale.

Mesure de la vitesse de rotation avec un codeur incrémental

Le codeur incrémental fournit deux signaux carrés en quadrature, comme sur la capture ci-dessous:

Ces deux signaux permettent de mesurer à la fois la vitesse et le sens de rotation.

Calcul de la vitesse

La mesure de la vitesse se fait simplement en comptant le nombre d’impulsions pendant un temps fixe. Les données du problème sont les suivantes :

  • Le codeur est fixé à l’arbre moteur et non pas à l’arbre de sortie du réducteur (celui utilisé pour l’entrainement). Le rapport de réduction étant 34:1, l’arbre moteur fait 34 tours lorsque l’arbre « principal » en fait 1
  • Le codeur génère 48 impulsions à chaque fois qu’il fait un tour
  • La cadence d’échantillonnage utilisée pour l’asservissement sera de 0.01 s

Par conséquent, lorsque l’arbre principal fait un tour, le codeur génère :
34 * 48 = 1632 impulsions.

Si N est le nombre d’impulsions comptées en 0.01 s, la vitesse est (en rad/s, l’unité standard, sachant qu’un tour fait 2*π radians) :
2*π*N/(0.01*1632)

Un point très important concerne la résolution de la mesure, c’est-à-dire la plus petite valeur qu’il est possible de calculer. La formule est la suivante (en rad/s) :
2*π/(Ts*CPR*ratio)
avec :

  • Ts : cadence d’échantillonnage
  • CPR : nombre d’impulsions par tour du codeur
  • ratio : rapport de réduction du moteur

Le but étant d’avoir la plus faible résolution possible (pour avoir une bonne précision de mesure), il faut avoir Ts et/ou CPR et/ou ratio le plus grand possible. Cependant, ratio est fixé par le besoin en couple ou en vitesse maximale alors que Ts doit être plus petit que le temps de réponse souhaité pour l’asservissement du moteur (voir plus loin). Par conséquent, la seule réelle possibilité est de jouer sur CPR. Lors du choix d’un codeur incrémental, il est préférable de prendre celui qui permet d’obtenir le plus d’impulsions par tour (si le budget le permet).

Dans notre cas de figure, la résolution est la suivante
2*π/(0.01*1632) = 0.4 rad/s

Ce n’est pas exceptionnel, le seul moyen de faire mieux serait de réduire le temps de réponse de l’asservissement de vitesse du moteur (voir plus loin).

Comptage du nombre d’impulsions

Compter le nombre d’impulsions du codeur revient à compter le nombre de fronts montants et descendants des signaux jaune et bleu représentés sur l’image ci-dessus. Pour ce faire, la seule méthode viable consiste à brancher les deux signaux (les fils jaune et blanc sur le codeur utilisé) sur deux entrées « interruption » de la carte Arduino. Les deux autres fils (bleu et vert) seront respectivement branchés sur le 5 V et sur la masse de l’Arduino.

Sur une carte Romeo (comme sur un Arduino Uno d’ailleurs), il y a deux lignes d’interruption (numérotées 0 et 1), qui correspondent aux broches digitales 2 et 3. L’intérêt d’une ligne d’interruption est qu’elle permet, comme son nom l’indique, d’interrompre le déroulement des calculs sur le micro-contrôleur pour effectuer un traitement spécifique, en l’occurrence la mise à jour du compteur d’impulsions, avant de rendre la main à la boucle principale.

La seule « difficulté » est de savoir s’il faut incrémenter ou décrémenter le compteur dans le traitement de l’interruption. Il suffit pour cela d’observer les courbes ci-dessus, obtenues alors que le moteur tourne dans le sens positif. On constate que:

  • Lorsque la voie A (en jaune) passe au niveau haut, la voie B (en bleu) est au niveau bas
  • Lorsque la voie A passe au niveau bas, la voie B est au niveau haut

Quand le moteur tourne dans le sens positif, lors d’une interruption sur la voie A, les niveaux de A et B sont donc inversés. Le code correspondant sur l’Arduino sera le suivant :

void GestionInterruptionCodeurPinA()
{
  if (digitalReadFast2(codeurPinA) == digitalReadFast2(codeurPinB)) {
    ticksCodeur--;
  }
  else {
    ticksCodeur++;
  }
}

En ce qui concerne l’interruption liée à la voie B, c’est l’inverse :

  • Lorsque la voie B passe au niveau haut, la voie A est au niveau haut
  • Lorsque la voie B passe au niveau bas, la voie A est au niveau bas

Le code de traitement de l’interruption de la voie B sera donc le suivant :

void GestionInterruptionCodeurPinB()
{
  if (digitalReadFast2(codeurPinA) == digitalReadFast2(codeurPinB)) {
    ticksCodeur++;
  }
  else {
    ticksCodeur--;
  }
}

Vous aurez peut-être remarqué l’utilisation de la fonction digitalReadFast2() pour lire le niveau des entrées codeur, au lieu de la fonction standard digitalRead(). Cette fonction, issues de la librairie digitalWriteFast (téléchargeable à l’adresse http://www.3sigma.fr/telechargements/digitalWriteFast.zip) permet d’améliorer la rapidité de lecture et d’écriture des entrées digitales pour minimiser le temps passé dans la routine d’interruption (et donc rendre la main plus vite à la boucle de calcul principal). Pour utiliser cette bibliothèque dans votre code, vous devez faire deux choses simples

  • décompresser l’archive que vous aurez téléchargée dans le sous-répertoire « librairies » de l’installation de votre environnement de développement Arduino
  • ajouter au début de votre programme la ligne
    #include <digitalWriteFast.h>

Le code permettant de calculer la vitesse est le suivant :

// Nombre de ticks codeur depuis la dernière fois
codeurDeltaPos = ticksCodeur;
ticksCodeur = 0;

// Calcul de la vitesse de rotation
omega = ((2.*3.141592*((double)codeurDeltaPos))/1632.)/0.01;  // en rad/s

Pour que cela fonctionne correctement, ce code doit être exécuté à la cadence fixe de 0.01 s. L’utilisation de la fonction delay() pour faire ça est une mauvaise méthode. En effet, vous pourriez penser écrire le code suivant :

void loop() {

  // Traitements divers
  // Lecture d'entrées + calculs par exemple

  // Attente de 10 ms
  delay(10);

}

Le problème est que les « traitements divers » vont prendre un certain temps, inconnu (T) et potentiellement variable (dans le cas où ils incluent des instructions conditionnelles). Par conséquent, la boucle sera exécutée toutes les T+0.01 s et non pas toutes les 0.01 s, et la mesure de la vitesse de rotation sera fausse.

La bonne méthode consiste à utiliser un timer du micro-contrôleur pour générer une interruption toutes les 0.01 s. Vous pouvez utiliser pour cela la bibliothèque FlexiTimer2 (téléchargeable ici: http://www.3sigma.fr/telechargements/FlexiTimer2.zip). Après l’avoir décompressée, il faut la placer dans le sous-répertoire « librairies » de l’installation de l’environnement de développement Arduino et ajouter au début du programme la ligne :

#include <FlexiTimer2.h>

Enfin, il faut ajouter les deux lignes suivantes dans la fonction startup() :

FlexiTimer2::set(10, 1/1000., isrt); // résolution timer = 1 ms
FlexiTimer2::start();

La première ligne crée une interruption qui se produit toutes les 10 ms (avec une résolution de 1 ms), ce qui exécute alors la fonction isrt() (contenant le code permettant de calculer la vitesse de rotation et de réaliser l’asservissement de vitesse). La seconde ligne démarre ce timer.

 

Asservissement en vitesse du moteur

Pour asservir le moteur en vitesse, nous allons utiliser un régulateur de type PID (Proportionnel Intégral Dérivé). Ce régulateur combine les 3 actions suivantes:

  • Proportionnelle : action proportionnelle à l’écart entre la consigne de vitesse et la mesure
  • Intégrale: cette action est basée sur l’intégrale par rapport au temps de l’écart entre la consigne et la mesure. Elle permet de compenser une erreur permanente (appelée « erreur statique ») entre la consigne et la mesure
  • Dérivée: cette action est basée sur la dérivée par rapport au temps de l’écart entre la consigne et la mesure. Elle permet d’anticiper cet écart, ce qui est utile pour des systèmes naturellement instables, au prix d’une amplification des bruits de mesure

Il n’est pas utile d’associer systématiquement ces trois actions (la proportionnelle est cependant toujours présente). Dans notre cas de figure, un PI suffira, l’anticipation apportée par l’action dérivée n’étant pas utile.

Les gains proportionnel et l’intégral étant respectivement Kp=0.29 et Ki=8.93, le code du la régulation PID s’écrit:

  /******* Régulation PID ********/
  // Ecart entre la consigne et la mesure
  ecart = vref - omega;

  // Terme proportionnel
  P_x = Kp * ecart;

  // Calcul de la commande
  commande = P_x + I_x;

  // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
  I_x = I_x + Ki * dt * ecart;
  /******* Fin régulation PID ********/

 

Code complet

Le code complet permettant de réaliser sur la carte Arduino Romeo cet asservissement de vitesse d’un moteur à courant continu avec codeur incrémental est le suivant :

#include <FlexiTimer2.h>
#include <digitalWriteFast.h> 

// Codeur incrémental
#define codeurInterruptionA 0
#define codeurInterruptionB 1
#define codeurPinA 2
#define codeurPinB 3
volatile long ticksCodeur = 0;

// Moteur CC
#define directionMoteur  4
#define pwmMoteur  5

// Cadence d'envoi des données en ms
#define TSDATA 100
unsigned long tempsDernierEnvoi = 0;
unsigned long tempsCourant = 0;

// Cadence d'échantillonnage en ms
#define CADENCE_MS 10
volatile double dt = CADENCE_MS/1000.;
volatile double temps = -CADENCE_MS/1000.;

volatile double omega;
volatile double commande = 0.;
volatile double vref = 3.14;

// PID
volatile double Kp = 0.29;
volatile double Ki = 8.93;
volatile double P_x = 0.;
volatile double I_x = 0.;
volatile double ecart = 0.;

// Initialisations
void setup(void) {

  // Codeur incrémental
  pinMode(codeurPinA, INPUT);      // entrée digitale pin A codeur
  pinMode(codeurPinB, INPUT);      // entrée digitale pin B codeur
  digitalWrite(codeurPinA, HIGH);  // activation de la résistance de pullup
  digitalWrite(codeurPinB, HIGH);  // activation de la résistance de pullup
  attachInterrupt(codeurInterruptionA, GestionInterruptionCodeurPinA, CHANGE);
  attachInterrupt(codeurInterruptionB, GestionInterruptionCodeurPinB, CHANGE);

  // Moteur CC
  pinMode(directionMoteur, OUTPUT);
  pinMode(pwmMoteur, OUTPUT);

  // Liaison série
  Serial.begin(9600);
  Serial.flush();

  // Compteur d'impulsions de l'encodeur
  ticksCodeur = 0;

  // La routine isrt est exécutée à cadence fixe
  FlexiTimer2::set(CADENCE_MS, 1/1000., isrt); // résolution timer = 1 ms
  FlexiTimer2::start();

}

// Boucle principale
void loop() {

  // Ecriture des données sur la liaison série
  ecritureData();

}

void isrt(){

  int codeurDeltaPos;
  double tensionBatterie;

  // Nombre de ticks codeur depuis la dernière fois
  codeurDeltaPos = ticksCodeur;
  ticksCodeur = 0;

  // Calcul de la vitesse de rotation
  omega = ((2.*3.141592*((double)codeurDeltaPos))/1632.)/dt;  // en rad/s

  /******* Régulation PID ********/
  // Ecart entre la consigne et la mesure
  ecart = vref - omega;

  // Terme proportionnel
  P_x = Kp * ecart;

  // Calcul de la commande
  commande = P_x + I_x;

  // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
  I_x = I_x + Ki * dt * ecart;
  /******* Fin régulation PID ********/

  // Envoi de la commande au moteur
  tensionBatterie = 7.2;
  CommandeMoteur(commande, tensionBatterie);

  temps += dt;
}

void ecritureData(void) {

  // Ecriture des données en sortie tous les TSDATA millisecondes
  tempsCourant = millis();
  if (tempsCourant-tempsDernierEnvoi > TSDATA) {
    Serial.print(temps);

    Serial.print(",");
    Serial.print(omega);

    Serial.print("\r");
    Serial.print("\n");

    tempsDernierEnvoi = tempsCourant;
  }
}

void CommandeMoteur(double tension, double tensionBatterie)
{
	int tension_int;

	// Normalisation de la tension d'alimentation par
        // rapport à la tension batterie
	tension_int = (int)(255*(tension/tensionBatterie));

	// Saturation par sécurité
	if (tension_int>255) {
		tension_int = 255;
	}
	if (tension_int<-255) {
		tension_int = -255;
	}

        // Commande PWM
	if (tension_int>=0) {
		digitalWrite(directionMoteur, LOW);
		analogWrite(pwmMoteur, tension_int);
	}
	if (tension_int<0) {
		digitalWrite(directionMoteur, HIGH);
		analogWrite(pwmMoteur, -tension_int);
	}
}

// Routine de service d'interruption attachée à la voie A du codeur incrémental
void GestionInterruptionCodeurPinA()
{
  if (digitalReadFast2(codeurPinA) == digitalReadFast2(codeurPinB)) {
    ticksCodeur--;
  }
  else {
    ticksCodeur++;
  }
}

// Routine de service d'interruption attachée à la voie B du codeur incrémental
void GestionInterruptionCodeurPinB()
{
  if (digitalReadFast2(codeurPinA) == digitalReadFast2(codeurPinB)) {
    ticksCodeur++;
  }
  else {
    ticksCodeur--;
  }
}

Ce code intègre ce qui a été détaillé précédemment.

Sotheby

Bonjour, je vais me lancer dans une régulation PID, comment as tu déterminé tes gains PI??? fondamentalement stp car mon appli est complétement différente.

Merci d’avance.

admin

@Sotheby: j’ai utilisé Maple et la Control Toolbox pour Maple (que j’ai développée): http://www.control-toolbox.com/fr-categorie-10-Accueil.html
La fonction de réglage de PID nécessite de connaître la fonction de transfert de ton système.
Tu as de la chance, je mets à disposition cette fonction en utilisation gratuite en ligne:
http://www.control-toolbox.com/store/lang-fr/reglage_pid.php

Si tu as des soucis, n’hésites pas. Tu peux me contacter directement à l’adresse suivante (remplacer at par @ et éliminer les espaces):
support at 3sigma . fr

Vincent

Bonjour,

Félicitation pour ton tutoriel qui est complet et synthétique !

Tu m’as fait découvrir la bibliothèque DigitalWriteFast qui doit s’avérer très utile pour le gain de temps. Cependant j’aimerais savoir si tu as testé ton code parce que il y a une erreur dans la bibliothèque que j’ai chargée… En effet la fonction reste soulignée en rouge (dans mon code) alors que les arguments sont correctes. Peux-tu m’aider à ce sujet ?

Cordialement

admin

Bonjour,

En effet, l’arborescence décompressée de la bibliothèque n’était pas utilisable telle quelle. J’ai modifié le lien de téléchargement (idem pour FlexiTimer2). Tu peux donc télécharger de nouveau, décompresser dans le dossier « libraries » de ton installation Arduino et ça va marcher.
Si tu as toujours des soucis, n’hésites pas à me demander.

Nicolas

Vincent

Bonjour,

Merci bien, je vais pouvoir regarder cela. Si un nouveau problème se pose je te tiens au courant.

Cordialement

Vincent

Bonjour,

Je viens d’essayer la bibliothèque digitalWriteFast mais elle ne fonctionne toujours pas. Les arguements sont pourtant corrects mais elle reste toujours soulignée. Je ne vois pas trop d’où peut venir le problème d’après le header…
Mais je vais utiliser une carte de comptage, car l’horloge de la MEGA 2560 n’est pas assez rapide pour compter toutes les impulsions d’après mes tests.
Je vais essayer FlexiTimer dès que je l’a reçoit et s’il y a un problème je te tiendrais au courant.

Cordialement

admin

Bonjour,

Où se trouve ton fichier digitalWriteFast.h ? Peux-tu me donner le chemin ? Normalement il doit être placé dans arduino-1.0/libraries/digitalWriteFast

Tu as une erreur à la compilation ? Si oui, laquelle ?

Merci d’avance pour les infos,

Nicolas

Vincent

Bonjour et désolé pour cette absence…

Le chemin est « C:\Program Files\Arduino\libraries », c’est la où je place toutes mes librairies.

L’erreur est la suivante :

« #define digitalReadFast2(P)((int)_digitalReadFast2_((P)))
Error : identificateur « _builtin_constant_p » non défini  »

Donc n’ayant pas non eu beaucoup de temps pour rechercher l’erreur, j’ai continué avec digitalRead. Le gain de temps est si important que ça ?

Bonne journée !!

admin

Bonjour Vincent,

Pour le gain de temps, tout dépend de ton besoin. Pour lire l’état d’un bouton poussoir, tu peux très largement t’en passer. Dans mon cas, c’est pour lire le niveau d’un signal codeur, il y a 1632 changements d’états par tour avec plusieurs tours par seconde, donc c’est intéressant d’aller le plus vite possible.
Je te suggère de faire une installation « fraiche » de la dernière version de l’IDE Arduino, de télécharger cette bibliothèque et de tester. Je viens d’essayer, ça a fonctionné de mon côté.

Bon courage,

Nicolas

Argenty

Bonjour,

Dans un premier temps je souhaite te féliciter pour le tuto.
Mais comme je débute dans le monde de l’arduino et l’électronique je voulais savoir si il était possible d’avoir le schéma de montage ?

Ce tuto pourra m’aider car je souhaite fabriquer un follow focus HF.

Merci d’avance 🙂

Luiggi

Je veux faire un asservissement de vitesse sur une carte UNO plus un shield Motor officiel.
Je commande la vitesse de rotation par un potentiomètre sans problème.
Quand j’utilise la librairie FlexiTimer2 pour mesurer la vitesse, la commande de vitesse du moteur (pwm) ne fonctionne plus : je suis soit a l’arrêt soit à la vitesse max.
Avez vous déjà rencontré ce type de problèmes?
La librairie FlexiTimer2 agit elle sur les timer qui gérent le pwm?
Cordialement et merci pour le travail réalisé

Louis

carouch

Bonjour,
Merci pour ce superbe tuto!!
Par contre, je n’arrive pas à savoir ce que je dois changer pour programmer par exemple que je veux 2tours à 20 tours par minute et 3 tours à 50tours par minute sans changer la valeur de la tensionbatterie.
Je ne sais pas si j’ai bien tout compris : vref=3.14 = 3.14*9.549296= 30 tr/min. tu veux donc que ton moteur tourne à cette cadence?
Merci pour ta réponse.
Bonne journée

admin

Bonjour,

Oui, vref est la vitesse de rotation souhaitée en rad/s. 3.14 correspond à un demi-tour par seconde, donc en effet à 30 tr/min.

Franck

Bonjour,

Je débute sur une base d’EasyRobotics avec deux servo moteur modifier en moteur CC driver par une carte
Ardumoto.

Les tolérances de fabrication des moteur font que mon Robot n’avance pas droit. Donc la premier chose à faire
je suppose et de faire un asservissement mais lequel?? En « vitesse » ou en « position »???

Apres recherche je n’est pas bien saisi la différence entre les deux.

Bonne journée.

admin

Bonjour Franck,

Lorsque le moteur est asservi en position, on lui donne une consigne d’angle (par exemple 50 degrés) et le moteur rejoint cette consigne. Si tu fais un asservissement de position sur ton robot, il ne va pas aller bien loin (le moteur va tourner de 50 degrés et s’arrêter).
Tu dois donc faire un asservissement de vitesse et donner par exemple une consigne de 50 degrés/secondes. Dans ce cas, le moteur va tourner à cette vitesse jusqu’à ce que tu lui dises de s’arrêter (consigne de 0 degrés/seconde).

Axel

Bonjour, merci pour ton tuto qui est très intéressant, ça m’aide beuacoup. Mais pourrais-tu me dire à quoi correspondent les fils bleu,vert,jaune et blanc?
Et est-ce que c’est possible d’avoir un schéma du montage? Merci d’avance.

Axel

admin

Sur le principe, oui, on peut aussi utiliser un Arduino classique associé à un shield de commande de moteur électrique.

Lombard

Bonjour
Super merci pour ce Tuto . Je suis sur un projet de gyropode
Type segway et je voudrais faire un asservissement des moteurs
Par contre ce sont des moteurs 24 v (my1016z) sans codeur
Puis je faire une régulation type pid sur ce projet ou cela n est pas obligatoire ?
Merci de votre aide

admin

Bonjour,

C’est faisable de faire de la régulation de vitesse sans codeur, mais c’est compliqué et peu précis. Il serait préférable d’ajouter un codeur (https://www.motiondynamics.com.au/hall-effect-encoder.html).
Ensuite, sur un gyropode, ce serait nettement mieux de pouvoir mesurer la vitesse du moteur, sinon il faut utiliser des moyens détournés très compliqués.
Bref, à mon sens il est nettement préférable d’ajouter un codeur sur les moteurs.

Bon courage !

Pierro

Bonjour,
Je test un autre moyen de faire un asservissement avec PID mais le principe reste presque identique, mon montage comprend un plateau tournant qui a une vis sans fin comme moyen de réduction. Vu que mon moteur s’arrête quasi instantanément lorsque ma consigne est à zéro, la régulation saccade trop. Connaissez vous un moyen de ne pas couper complètement la puissance en cas de dépassement de la consigne ???

Merci d’avance et joli tuto 🙂

admin

Bonjour,

Est-ce que le problème ne viendrait pas du fait qu’il y a une zone morte sur une moteur à courant continu ? Par exemple, même sans charge, le moteur ne va pas tourner si on lui applique une tension comprise entre -0.5V et 0.5V.
Peut-être que l’utilisation d’un moteur pas-à-pas pour faire ce type de contrôle en position donnerait de meilleurs résultats ?

Cordialement,

Nicolas

David

Bonjour,

Je viens de faire l’acquisition d’une carte Mega2560.
J’ai épuré votre code pour mesurer l’angle et la vitesse à partir d’un codeur incrémental à effet hall.
Tout marche très bien.

J’ai décidé de ne plus utiliser les broches digitaux 2 et 3 (dans l’idée d’utiliser 2 codeurs par la suite) mais les ports 18 et 19.
J’ai changé mes branchements et le code (sans trop savoir quoi mettre à codeurInterruptionA et codeurInterruptionB)
#define codeurInterruptionA 0
#define codeurInterruptionB 1
#define codeurPinA 18
#define codeurPinB 19

Cela ne marche plus. Pourriez vous m’aider ?

David

Bonjour,

N’ayant pas la possibilité d’éditer mon précédent message, je le complète avec celui ci : Il me semble avoir trouvé pour codeurInterruption :
Broche 2 : interrupt 0
Broche 3 : interrupt 1
Broche 18 : interrupt 5
Broche 19 : interrupt 4
Broche 20 : interrupt 3
Broche 21 : interrupt 2
Mais rien n’y fait

admin

Bonjour,

Normalement ce doit être:
#define codeurInterruptionA 5
#define codeurInterruptionB 4
#define codeurPinA 18
#define codeurPinB 19

Pour info, sur le robot T-Quad (http://www.robot-tquad.com/fr) qui est basé sur une Mega2560, nous utilisons la bibliothèque EnableInterrupt (http://www.3sigma.fr/telechargements/EnableInterrupt.zip) qui permet d’écrire les choses plus simplement, sans différencier le numéro de la broche et le numéro de l’interruption. Par exemple, cela donne:

#define codeurAvantGauchePinA 18

void setup(void) {

enableInterrupt(codeurAvantGauchePinA, GestionInterruptionCodeurAvantGauchePinA, CHANGE);

}

Cordialement,

Nicolas

David

En utilisant ce code, j’ai pu de problème. Plus besoin de chercher la relation entre la broche digitale et le numéro d’interruption.
#define codeurPinA 18
#define codeurPinB 19
#define codeurInterruptionA digitalPinToInterrupt(codeurPinA)
#define codeurInterruptionB digitalPinToInterrupt(codeurPinB)

Hugo Desrutins

Bonjour, j’utilise votre programme d’asservissement sur un Moteur a courant continu de type EMG30, le programme tourne et l’asservissement semble correct cependant lorsque je met une consigne Vref = 18 (ce qui correspond a 170 t/min vitesse nominale de mon Moteur), le moniteur série m’indique une vitesse = 0.
Savez vous d’ou vient ce problème ?

Merci d’avance.

momo

SVP cher admin j’a un souci , je veux créer un régulateur PID aussi ( projet de fin etude) , la commande ou ma correction calculée ( la somme de P I D ) tu l’as multiplier par 255 comme ca :
(255*(tension/tensionBatterie)); je pense ici que notre coorection tu l’as nomé « tension  » n’est ce pas ?
s’il te plait j’ai besoin de me clarifier ce ponit

admin

« tension » est la tension de commande qui doit être appliquée au moteur. Mais celui-ci est commandé par un PWM, dont le rapport cyclique doit être compris entre 0 (rapport cyclique de 0%) et 255 (rapport cyclique de 100%).
Ce pourcentage est un pourcentage de la tension d’alimentation du driver moteur, abusivement appelée ici « tensionBatterie ». En effet, en toute rigueur, la tension d’alimentation du driver moteur est inférieure à la tension d’alimentation générale du système (adaptateur secteur ou batterie).
Quoiqu’il en soit, tension/tensionBatterie correspond à ce pourcentage, qu’il faut donc multiplier par 255 pour avoir la valeur à envoyer à la fonction analogWrite qui génère le PWM.

lacazol

Bonjour et merci pour votre post. Je ne comprends par contre pas comment vous réalisez l’asservissement sur les deux roues alors qu’on ne peut mesurer la vitesse de rotation d’une seule de celles-ci étant donné que les lignes d’interruptions sont déjà occupées. Je ne parviens pas à imposer la vitesse désirée même avec votre programme. Si vous pouviez m’aider ce serait super

admin

Quand on a 2 moteurs, on utilise une seule ligne d’interruption par moteur. On branche la ligne A du codeur sur cette interruption et la ligne B sur une entrée digitale classique. La résolution du codeur est ainsi divisée par 2 mais ça fonctionne quand même.
On a toujours deux routines d’interruption en tout, une par moteur: GestionInterruptionCodeurMoteurDroitPinA() et GestionInterruptionCodeurMoteurGauchePinA()

lacazol

D’accord c’est ce que j’ai voulu faire mais mon calcul de vitesse de rotation est alors faux quand je fais apparaître les valeurs de vitesse, ou bien ce sont mes gestioninterruption qui sont faux… Les fonctions gestioninterruption ne sont alors plus les mêmes ?

admin

Ca dépend à quel point c’est faux. Si c’est un facteur 2 ou un signe qui change, c’est facile à corriger.