Aller au contenu


Photo
* * * * * 2 note(s)

Arduino due, Moteurs CC et asservissement PID

Arduino Due Codeurs PID Asservissement

  • Veuillez vous connecter pour répondre
33 réponses à ce sujet

#1 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 924 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 18 mars 2019 - 12:27

Puisque c'est une question qui revient souvent concernant ce tutoriel : 
 

Mise en place d'un PID sur une arduino uno/nano  présenté aussi dans ce sujet dans le cadre de la mise en place d'un asservissement sur un robot 2WD pour un robot pour la coupe de france de robotique

 

J'ouvre donc ce sujet pour traiter de la modification du programme PID fourni dans les liens ci dessus écrit pour arduino uno / nano pour adaptation pour arduino Due : 

 

Les points important à souligner sont : 

  •  Modification de la librairie utilisée pour les Timers :  TimerOne utilisé dans les code précédent était une librairie qui n'était pas compatible avec l'arduino Due => Utilisation de la librairie DueTimer
     
  • Tous les pins 2 à 13 sont des pins PWM de la due  
     
  • Modification du code pour la lecture des codeurs => PIND est valable que pour nano et uno tel que défini dans le précédent code ...
     
  • Tous les pins de la due peuvent être utilisé en interruption :  => On peut donc facilement doubler la résolution sur les codeurs et on ne va pas s'en priver
     
  • L'arduino due étant beaucoup plus performante que l'arduino uno ou mega on va pouvoir contrôler de nombreux moteurs CC. On ne va donc pas s'en priver et on va faire un code capable de contrôler plusieurs moteurs, qu'on pourra facilement modifier pour changer le nombre de moteurs 
     
  • Kit à joindre l'utile à l'agreable et puisqu'on est aussi un peu faignant ( oui ça a parfois du bon ) on ferra en sorte d'éviter au maximum de dupliquer du code, et on utilisera des notions avancées tels que des tableaux d'objets et des pointeurs sur fonction. 

Code  en plusieurs étapes à venir : 

 


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#2 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 504 messages
  • Gender:Male
  • Location:Paris

Posté 18 mars 2019 - 08:09

J'ai beaucoup aimé le bidouiller ce Due ;)

Merci pour l'info.



#3 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 924 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 19 mars 2019 - 01:18

Voici un premier jet de code pour afficher les résultats des mesures de "vitesse" via 5 codeurs . 

 

#include <DueTimer.h> // Pour gérer les interruptions sur Timer


// Constantes à modifier
#define NBMOTEURS 5
const uint8_t pinEncodeurA[NBMOTEURS]={32,34,36,38};
const uint8_t pinEncodeurB[NBMOTEURS]= {33,35,37,39};


// Volatile : Variables modifiées dans les interruptions 
volatile int16_t compteur[NBMOTEURS]={0,0,0,0,0};
volatile bool dernierEtatEncodeurA[NBMOTEURS]={0,0,0,0,0};
volatile bool dernierEtatEncodeurB[NBMOTEURS]={0,0,0,0,0};
volatile bool flagTimer3 = 0;  // indique que l'interruption sur timer s'est effectuée

//Fonction de comptage des interruptions 

// Fonction générique 
void compteA( uint8_t i){
  bool etatA = digitalRead(pinEncodeurA[i]);
  (etatA ^ dernierEtatEncodeurB[i]) ? compteur[i]++ :compteur[i]--;
  dernierEtatEncodeurA[i]=etatA;
}

void compteB( uint8_t i){
  bool etatB = digitalRead(pinEncodeurB[i]);
  (etatB ^ dernierEtatEncodeurA[i]) ? compteur[i]-- : compteur[i]++;
  dernierEtatEncodeurB[i]=etatB;
}

// Fonctions d'interruption spécifique pour chaque pin codeur A
void compteA0() {
  compteA(0);
}
void compteA1() {
  compteA(1);
}
void compteA2() {
  compteA(2);
}
void compteA3() {
  compteA(3);
}
void compteA4() {
  compteA(4);
}

void compteB0() {
  compteB(0);
}
void compteB1() {
  compteB(1);
}
void compteB2() {
  compteB(2);
}
void compteB3() {
  compteB(3);
}
void compteB4() {
  compteB(4);
}


// Tableau de pointeurs sur fonction pour faciliter l'usage des interruptions
void (*compteEncodeurA[])(void)= {compteA0,compteA1,compteA2,compteA3,compteA4};
void (*compteEncodeurB[])(void)= {compteB0,compteB1,compteB2,compteB3,compteB4};


// Fonction timer
void timer3Interrupt() {
  for(uint8_t i = 0; i<NBMOTEURS; i++) {
    vitesse[i] = compteur[i];
    compteur[i] = 0;
  }
  flagTimer3 = 1;
}

void setup() {

  Serial.begin(115200); 
  
  for(uint8_t i = 0; i < NBMOTEURS; i++){

   // Initialisation des codeurs
   pinMode(pinEncodeurA[i], INPUT_PULLUP);
   pinMode(pinEncodeurB[i], INPUT_PULLUP);
   attachInterrupt(digitalPinToInterrupt(pinEncodeurA[i]), compteEncodeurA[i], CHANGE);
   attachInterrupt(digitalPinToInterrupt(pinEncodeurB[i]), compteEncodeurB[i], CHANGE);
  }

  // Initialisation du timer
  Timer3.attachInterrupt(timer3Interrupt);
  Timer3.start(50000); // Calls every 50ms

  // Initialisation du PID
}

void loop() {
  
  if( flagTimer3 ) {
    for(uint8_t i = 0; i < NBMOTEURS; i++) {
      Serial.print(vitesse[i]);
      Serial.print(",");    
     }
     Serial.println("");
     flagTimer3 = 0;
  }
}

Avant d'aller plus loin en intégrant les fonctions de contrôle des moteurs et l'implémentation des PID on s'assure déjà que l'acquisition des mesures fonctionnes correctement . 

 

Il est possible d'observer les résultats avec le moniteur série arduino et le plotter série pour les afficher sous forme de graphique.


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#4 bvking

bvking

    Membre occasionnel

  • Membres
  • Pip
  • 98 messages

Posté 19 mars 2019 - 01:35

Ok Jonathan,

 

Test de la première étape OK !

 

Toutes les vitesses sont lues correctement.

J'ai les données des vitesses positives dans un sens et négatives de l'autre.



#5 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 924 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 19 mars 2019 - 03:47

Deuxième étape on ajoute le contrôle des moteurs : 

=> Initialisation de pins, mis en place des fonctions de pilotage des moteurs par pwm et vérification des sens de rotation : 
 

#include <DueTimer.h> // Pour gérer les interruptions sur Timer

// Constantes à modifier

#define NBMOTEURS 5

// Pins des moteurs 
const uint8_t pinVitesseMoteur[NBMOTEURS] = {3,4,5,6,7};
const uint8_t pinSensAMoteur[NBMOTEURS] = {22,24,26,28,30};
const uint8_t pinSensBMoteur[NBMOTEURS] = {23,25,27,29,31};

//Pins des codeurs
const uint8_t pinEncodeurA[NBMOTEURS] = {32,34,36,38};
const uint8_t pinEncodeurB[NBMOTEURS] = {33,35,37,39};


// Variables globales

//Variables modifiées dans les interruptions => "volatile"
volatile int16_t compteur[NBMOTEURS] = {0,0,0,0,0};
volatile double vitesse[NBMOTEURS] = {0,0,0,0,0};
volatile bool dernierEtatEncodeurA[NBMOTEURS] = {0,0,0,0,0};
volatile bool dernierEtatEncodeurB[NBMOTEURS] = {0,0,0,0,0};
volatile bool flagTimer3 = 0;  // indique que l'interruption sur timer s'est effectuée


//Fonction de comptage des interruptions 

// Fonction générique 
void compteA( uint8_t i) {
  bool etatA = digitalRead(pinEncodeurA[i]);
  (etatA ^ dernierEtatEncodeurB[i]) ? compteur[i]++ :compteur[i]--;
  dernierEtatEncodeurA[i] = etatA;
}

void compteB( uint8_t i) {
  bool etatB = digitalRead(pinEncodeurB[i]);
  (etatB ^ dernierEtatEncodeurA[i]) ? compteur[i]-- : compteur[i]++;
  dernierEtatEncodeurB[i] = etatB;
}

// Fonctions d'interruption spécifique pour chaque pin codeur A
void compteA0() {
  compteA(0);
}
void compteA1() {
  compteA(1);
}
void compteA2() {
  compteA(2);
}
void compteA3() {
  compteA(3);
}
void compteA4() {
  compteA(4);
}

void compteB0() {
  compteB(0);
}
void compteB1() {
  compteB(1);
}
void compteB2() {
  compteB(2);
}
void compteB3() {
  compteB(3);
}
void compteB4() {
  compteB(4);
}


// Tableau de pointeurs sur fonction pour faciliter l'usage des interruptions
void (*compteEncodeurA[])(void)= {compteA0,compteA1,compteA2,compteA3,compteA4};
void (*compteEncodeurB[])(void)= {compteB0,compteB1,compteB2,compteB3,compteB4};

// Fonction timer
void timer3Interrupt() {
  for(uint8_t i = 0; i<NBMOTEURS; i++) {
    vitesse[i] = compteur[i];
    compteur[i] = 0;
  }
  flagTimer3 = 1;
}

// fonction de pilotage des moteurs (à modifier en fonction du driver de moteur utilisé)

void setMoteur(uint8_t moteur, int16_t vitesse) {

  // Sécurités optionnelles pour s'assurer que les entrées sont bien dans les plages attendues...
  if(moteur >= NBMOTEURS) 
   return; 
  vitesse = constrain(vitesse,-255, 255);  // le PWM est sur 8 bits et ne va que de 0 à 255. 
  // Fin des sécurités optionnelles. 

  if(vitesse == 0 ) {
    // Frein d'urgence ou roue libre ... 
    // à configurer en fonction de ce que la carte moteur permet de faire..
    digitalWrite(pinSensAMoteur[moteur], HIGH);
    digitalWrite(pinSensBMoteur[moteur], HIGH);
  }
  if(vitesse > 0) {
    //Sens de contrôle indiqué comme positif par les codeurs
    digitalWrite(pinSensAMoteur[moteur], HIGH);
    digitalWrite(pinSensBMoteur[moteur], LOW);
  } else {
    //Sens de contrôle indiqué comme négatif par les codeurs
    digitalWrite(pinSensAMoteur[moteur], LOW);
    digitalWrite(pinSensBMoteur[moteur], HIGH);
  }
  analogWrite(pinVitesseMoteur[moteur], abs(vitesse)); // écriture du PWM 
}

void testMoteurs( int16_t vitesse) {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    setMoteur(i, vitesse);
  }
}

void setup() {

  Serial.begin(115200); 
  
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    
   // Initialisation des pins moteurs
   pinMode(pinVitesseMoteur[i], OUTPUT);
   digitalWrite(pinVitesseMoteur[i], LOW);
   pinMode(pinSensAMoteur[i], OUTPUT); 
   digitalWrite(pinSensAMoteur[i], LOW);
   pinMode(pinSensBMoteur[i], OUTPUT);
   digitalWrite(pinSensBMoteur[i], LOW);

   // Initialisation des codeurs
   pinMode(pinEncodeurA[i], INPUT_PULLUP);
   pinMode(pinEncodeurB[i], INPUT_PULLUP);
   attachInterrupt(digitalPinToInterrupt(pinEncodeurA[i]), compteEncodeurA[i], CHANGE);
   attachInterrupt(digitalPinToInterrupt(pinEncodeurB[i]), compteEncodeurB[i], CHANGE);
  }

  // Initialisation du timer
  Timer3.attachInterrupt(timer3Interrupt);
  Timer3.start(50000); // Calls every 50ms

  // Initialisation du test des moteurs => Attention les moteurs vont se mettre à tourner brusquement
  testMoteurs(127); // PWM à 50%
}

void loop() {
  
  if( flagTimer3 ) {
    for(uint8_t i = 0; i < NBMOTEURS; i++) {
      Serial.print(vitesse[i]);
      Serial.print(",");    
     }
     Serial.println("");
     flagTimer3 = 0;
  }
}

Il faut vérifier que les moteurs tournent bien dans le sens "Positif souhaité " et que les codeurs indiquent bien une vitesse positive. 
En remplaçant 

 

" testMoteurs(127); " par " testMoteurs(-127); " les moteurs doivent tourner dans le sens contraire et les codeurs doivent indiquer une vitesse négative ... 


=> On va pouvoir ajuster le Timer en fonction des valeurs de vitesse affichées pour ce PWM à 50%


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#6 bvking

bvking

    Membre occasionnel

  • Membres
  • Pip
  • 98 messages

Posté 20 mars 2019 - 11:41

Salut Jonathan.

 

Je n'ai pas encore branché les moteurs 4 et 5 aux drivers, mais j'ai branché les encodeurs.

 

J'ai donc fait les tests sur 3 moteurs, en marche avant 127  et marche arrière -127.

 

Et les 3 moteurs fonctionnent, j'ai bien les données qui vont respectivement en sens positif et negative.

 

Voilà quelques données pour testMoteurs(127)

 

187.00,192.00,170.00,0.00,0.00,
186.00,192.00,184.00,0.00,0.00,
187.00,193.00,173.00,0.00,0.00,
 

et pour -127
 
-184.00,-195.00,-183.00,0.00,0.00,
-183.00,-196.00,-185.00,0.00,0.00,
-184.00,-195.00,-180.00,0.00,0.00,
-185.00,-195.00,-181.00,0.00,0.00,
 
Merci


#7 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 924 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 20 mars 2019 - 12:36

 

Salut Jonathan.

 

Je n'ai pas encore branché les moteurs 4 et 5 aux drivers, mais j'ai branché les encodeurs.

 

J'ai donc fait les tests sur 3 moteurs, en marche avant 127  et marche arrière -127.

 

Et les 3 moteurs fonctionnent, j'ai bien les données qui vont respectivement en sens positif et negative.

 

Voilà quelques données pour testMoteurs(127)

 

187.00,192.00,170.00,0.00,0.00,
186.00,192.00,184.00,0.00,0.00,
187.00,193.00,173.00,0.00,0.00,
 

et pour -127
 
-184.00,-195.00,-183.00,0.00,0.00,
-183.00,-196.00,-185.00,0.00,0.00,
-184.00,-195.00,-180.00,0.00,0.00,
-185.00,-195.00,-181.00,0.00,0.00,
 
Merci

 

 

 

Ok vu qu'on a PWM et valeur mesurée qui sont du même ordre de grandeur on est pas trop mal réglé au niveau du timer... 
Mais on peut ajuster un peu le timer.

 

 

Notions théoriques : 

En général on préfère pouvoir "régler la consigne au moins aussi finement" qu'on mesure notre sorte ... pour éviter d'avoir des "effets yoyo" entre deux consignes ... 

Exemple concret. 

On peut régler le PWM de 0 à 255. Imaginons qu'on mesure  une vitesse max de 510 sur un moteur donné  pour un pwm de 255 et que le système est "parfait" 

( parfait => Lien proportionnel entre PWM et vitesse mesuré qui reste constant. ) 

 

Dans ce cas
Si je demande une vitesse de 0 => Pwm 0 .  ok . 
Si je demande une vitesse de 2 => Pwm 1 : ok. 

Mais si je demande une vitesse de 1 ? => Pwm  va varier entre 0 et 1 ... avec vitesse qui varie entre 0 et 2  => effet yoyo

 

=> Problème d'échantillonnage : on mesure une erreur qu'on est pas capable de corriger... 

 

Alors que si on a le contraire : 

On peut régler le PWM de 0 à 255. mais mesure max 127 sur un moteur donné  pour un pwm de 255 et que le système est "parfait" 

 

si je veux une vitesse de 1 => pwm  2 
                       vitesse de 2 => pwm  4 

                       ...

                       vitesse 127 => pwm 254

 

là on a l'effet contraire : la commande est sur échantillonné par rapport à la mesure ...  On perd de la résolution de consigne ... 
En effet on ne pourra désormais piloter que 127 vitesses différentes et on a de l'erreur qu'on ne voit pas ... 

Qu'on mette un pwm de 2 ou 3 sur 255 on lira une vitesse de 1,  alors qu'il y a une différence de vitesse entre les deux ... Mais que la résolution de notre acquisition ne le permet pas ... 

 

 

Donc en gros on essaye d'avoir à peu près un rapport de 1 entre vitesse max mesurée et consigne max pwm ... 

 

Du coup au vu des mesures donnée on peut essayer de raccourcir un peu la période de notre timer à  40ms . 
Et vérifier si on a bien un rapport de 1 à peu près entre consigne et pwm avec le programme de test.

Pour cela ne  pas hésiter à tester avec plusieurs valeurs 

 

exemple : 

testMoteurs(10)

testMoteurs(100)

testMoteurs(200)

testMoteurs(255)

 

 

 

 


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#8 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 924 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 20 mars 2019 - 02:06

Une fois notre période d'acquisition bien réglée, on peut ajouter les asservissements PID: 

#include <DueTimer.h> // Pour gérer les interruptions sur Timer
#include <PID_v1.h>  // Pour le PID

/* 
 * Constantes Globales à modifier
 */

#define NBMOTEURS 5

// Pins des codeurs
const uint8_t PINENCODEURA[NBMOTEURS] = {32,34,36,38};
const uint8_t PINENCODEURB[NBMOTEURS] = {33,35,37,39};

// Timers
const uint16_t PERIODE = 40000; // période du Timer3 en µs qui exécute le calcul des vitesses

// Pins des moteurs 
const uint8_t PINVITESSEMOTEUR[NBMOTEURS] = {3,4,5,6,7};
const uint8_t PINSENSAMOTEUR[NBMOTEURS] = {22,24,26,28,30};
const uint8_t PINSENSBMOTEUR[NBMOTEURS] = {23,25,27,29,31};

// PID => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const double KP = 0.9, KI = 0.4, KD = 0; 

/*
 * Variables globales 
 */

// Variables modifiées dans les interruptions => "volatile"
volatile int16_t compteur[NBMOTEURS] = {0,0,0,0,0};  // nombre de ticks compté
volatile int16_t vitesse[NBMOTEURS] = {0,0,0,0,0};    // nombre de ticks par unité de temps PERIODE3 
volatile bool dernierEtatEncodeurA[NBMOTEURS] = {0,0,0,0,0};
volatile bool dernierEtatEncodeurB[NBMOTEURS] = {0,0,0,0,0};
volatile bool flagTimer3 = 0;  // indique que l'interruption sur timer s'est effectuée

// Variables de PID ("double" car c'est la librairie utilisée qui veut ça ... )
double setpoint[NBMOTEURS] = {0,0,0,0,0};
double input[NBMOTEURS] = {0,0,0,0,0};
double output[NBMOTEURS] = {0,0,0,0,0};

// Tableau de pointeurs sur objets PID
PID PIDMOTEUR[NBMOTEURS] = {
  PID (&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT),
  PID (&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT),
  PID (&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT),
  PID (&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT),
  PID (&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT)
};

// Autre méthode d'écriture
/*PID PID0(&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT);
PID PID1(&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT);
PID PID2(&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT);
PID PID3(&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT);
PID PID4(&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT);

PID PIDMOTEUR[NBMOTEURS]= {PID0,PID1, PID2, PID3, PID4};*/


/* 
 * Fonctions  
 */


//Fonction de comptage des interruptions 

// Fonction de comptage générique 
void compteA( uint8_t i) {
  bool etatA = digitalRead(PINENCODEURA[i]);
  (etatA ^ dernierEtatEncodeurB[i]) ? compteur[i]++ :compteur[i]--;
  dernierEtatEncodeurA[i] = etatA;
}

void compteB( uint8_t i) {
  bool etatB = digitalRead(PINENCODEURB[i]);
  (etatB ^ dernierEtatEncodeurA[i]) ? compteur[i]-- : compteur[i]++;
  dernierEtatEncodeurB[i] = etatB;
}

// Fonctions d'interruption spécifique pour chaque pin codeur A et B
void compteA0() {
  compteA(0);
}
void compteA1() {
  compteA(1);
}
void compteA2() {
  compteA(2);
}
void compteA3() {
  compteA(3);
}
void compteA4() {
  compteA(4);
}

void compteB0() {
  compteB(0);
}
void compteB1() {
  compteB(1);
}
void compteB2() {
  compteB(2);
}
void compteB3() {
  compteB(3);
}
void compteB4() {
  compteB(4);
}

// Tableau de pointeurs sur fonction pour faciliter l'usage des interruptions
void (*compteEncodeurA[])(void)= {compteA0,compteA1,compteA2,compteA3,compteA4};
void (*compteEncodeurB[])(void)= {compteB0,compteB1,compteB2,compteB3,compteB4};

// Fonction timer
void timer3Interrupt() {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    vitesse[i] = compteur[i];
    compteur[i] = 0;
  }
  flagTimer3 = 1;
}

// fonction de pilotage des moteurs (à modifier en fonction du driver de moteur utilisé)
void setMoteur(uint8_t moteur, int16_t vitesse) {

  // Sécurités optionnelles pour s'assurer que les entrées sont bien dans les plages attendues...
  if(moteur >= NBMOTEURS) 
   return; 
  vitesse = constrain(vitesse,-255, 255);  // le PWM est sur 8 bits et ne va que de 0 à 255. 
  // Fin des sécurités optionnelles. 

  if(vitesse == 0 ) {
    // Frein d'urgence ou roue libre ... 
    // à configurer en fonction de ce que la carte moteur permet de faire..
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH);
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH);
  }
  if(vitesse > 0) {
    //Sens de contrôle indiqué comme positif par les codeurs
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH);
    digitalWrite(PINSENSBMOTEUR[moteur], LOW);
  } else {
    //Sens de contrôle indiqué comme négatif par les codeurs
    digitalWrite(PINSENSAMOTEUR[moteur], LOW);
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH);
  }
  analogWrite(PINVITESSEMOTEUR[moteur], abs(vitesse)); // écriture du PWM 
}

// Fonction de test qui fait tourner tous les moteur dans le même sens avec le même PWM
void testMoteurs( int16_t vitesse) {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    setMoteur(i, vitesse);
  }
}

// Fonctions asservissement PID 

void asservissementMoteur(uint8_t moteur) {
  input[moteur] = vitesse[moteur];
  PIDMOTEUR[moteur].Compute();
  setMoteur(moteur, (int)output[moteur]);
}

void asservissementMoteurs() {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    asservissementMoteur(i);
  }
}

// Fonction de conversion des unités 

void setup() {

  Serial.begin(115200); 
  
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    
   // Initialisation des pins moteurs
   pinMode(PINVITESSEMOTEUR[i], OUTPUT);
   digitalWrite(PINVITESSEMOTEUR[i], LOW);
   pinMode(PINSENSAMOTEUR[i], OUTPUT); 
   digitalWrite(PINSENSAMOTEUR[i], LOW);
   pinMode(PINSENSBMOTEUR[i], OUTPUT);
   digitalWrite(PINSENSBMOTEUR[i], LOW);

   // Initialisation des codeurs
   pinMode(PINENCODEURA[i], INPUT_PULLUP);
   pinMode(PINENCODEURB[i], INPUT_PULLUP);
   attachInterrupt(digitalPinToInterrupt(PINENCODEURA[i]), compteEncodeurA[i], CHANGE);
   attachInterrupt(digitalPinToInterrupt(PINENCODEURB[i]), compteEncodeurB[i], CHANGE);
 
   // Initialisation des PID 
   PIDMOTEUR[i].SetMode(AUTOMATIC);
   PIDMOTEUR[i].SetOutputLimits(-255, 255);

  }

  // Initialisation du timer
  Timer3.attachInterrupt(timer3Interrupt);
  Timer3.start(PERIODE); 

  
  //testMoteurs(127);

  // Initialisation des setpoints pour test d'asservissement
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
      setpoint[i] = 127;
  }
  
}

void loop() {
  
  if(flagTimer3) {
    asservissementMoteurs();     
    flagTimer3 = 0;

    // Affichage utile uniquement pour debugage
    for(uint8_t i = 0; i < NBMOTEURS; i++) {
      Serial.print(vitesse[i]);
      Serial.print(",");    
    }
    Serial.println("");
  }
  
}

Normalement maintenant les sorties des moteurs doivent tendre vers la valeur de setpoint donnée => 127 dans notre cas 

=> Il faudra alors tuner les paramètres PID pour avoir un bon comportement. 

C'est là que prend tout l'intérêt d'utiliser le plotter série intégré dans arduino pour afficher ces valeurs sous forme de courbes =)
=> Plus facile pour ajuster le PID


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#9 bvking

bvking

    Membre occasionnel

  • Membres
  • Pip
  • 98 messages

Posté 20 mars 2019 - 06:17

J'ai mis la période du timer à 35 ms.

 

Avec ce réglage à 127, j'ai quasi du 1:1 pour les 4 moteurs.

 

à -127

-126.00,-138.00,-125.00,-136.00,0.00,

-126.00,-137.00,-125.00,-135.00,0.00,

 

Et pour les autres

 

 

à 255

178.00,174.00,162.00,172.00,0.00,

173.00,174.00,164.00,173.00,0.00,

 

à 200

155.00,157.00,149.00,165.00,0.00,

165.00,159.00,152.00,162.00,0.00,

 

à -100

 

-99.00,-116.00,-108.00,-122.00,0.00,

-102.00,-116.00,-107.00,-121.00,0.00,

-95.00,-116.00,-107.00,-123.00,0.00,

 

à 100

 

100.00,113.00,108.00,117.00,0.00,

78.00,114.00,107.00,118.00,0.00,

86.00,113.00,109.00,117.00,0.00,

 

À -60

-40.00,-63.00,-63.00,-68.00,0.00,

-43.00,-66.00,-64.00,-71.00,0.00,

-35.00,-62.00,-62.00,-68.00,0.00,

 

À 55

29.00,54.00,48.00,61.00,0.00,

28.00,53.00,48.00,60.00,0.00,

26.00,53.00,49.00,57.00,0.00,

 
Les moteurs 1 et 4 sont branchés sur le même driver.
Les moteurs 2 et 3 sont branchés sur un autre driver.
 
On voit clairement qu'à partir du PWM 100 et en deçà (-60 et 55), le moteur 1 va significativement moins vite que les autres.
 
J'ai mesuré la tension pour les moteurs 1, 4 et 2, 3 et j'ai ceci (les deux drivers sont alimentés indépendamment en 6V).
 
2.63, 3.06 et 2.93, 2.83.
 
Ce qui à l'air cohérent pour les moteurs 2,3,4 mais pour 1?
 
En testant le PWM à 127 j'ai
 
3.25, 3.55 et 3.45, 3.35.
 
Là aussi on voit que le moteur 1 est bien moins alimenté que le 4. 
Tandis que les moteurs 2 et 3 ont quasi la même tension.
 
Dois-je changer mon premier driver? 
 
Je pensais qu'à 127 le PWM envoyait 50% de la tension de l'alimentation soit 3V
 
Est ce normal d'être autour de 3.3 et 3.6V?
 
PS: je vais quand même tester le programme avec PID.


#10 bvking

bvking

    Membre occasionnel

  • Membres
  • Pip
  • 98 messages

Posté 21 mars 2019 - 02:14

Bonjour,

 

J'ai teste l'asservissement avec Kp=1 Ki=10 kd=0.01 avec une vitesse de 100 avec plusieurs timers

 

35 ms

 

100,100,100,99,0,

102,99,101,101,0,
101,98,97,101,0,
100,99,99,101,0,
99,99,100,101,0,
98,101,102,100,0,
 
Les moteurs sont alimentés à 2.5 volt à + ou - 0.03 V, on voit qu'ils oscillent tous entre 2 et 3% autour de leurs vitesses asservies
 
à 50 ms
 
107,96,96,101,0,
108,94,99,98,0,
96,98,101,97,0,
98,101,110,101,0,
100,102,102,103,0,
104,103,102,102,0,
 
les moteurs sont alimentés à 1.85 V à + ou - 0.06 v 
Ils vont donc moins vite (ce qui est plus joli pour mon projet artistique), et l'asservissement est encore plus instable. 
 
 
 

à 20 ms

100,99,94,100,0,

101,99,95,100,0,
99,99,93,100,0,
98,99,95,100,0,
99,99,94,100,0,
101,98,95,100,0,
 
Ici l'asservissement relatif à chaque moteur est plus stable qu'à 35 ms, mais différents les uns des autres!!!
Pourquoi ne sont 'ils pas tous asservis à 100?
 
Les moteurs 2 et 3 sont alimentés à 4,33 et 4,34 volt et la tension ne varie pas du tous ! Les moteurs 1 et 4 leurs tensions bougent de 0.015volt, donc quasi stable aussi!!!
 
 
Mais on voit à l'oeil nul que les moteurs ne tournent pas à la même vitesse puisqu'ils se dephasent tous le temps.
 
Comme ils vont trop vites, j'ai réglé la vitesse à 30 mais l'asservissement oscille beaucoup trop et les tensions varient de 0.25 volts! 
 
Le but est que les moteurs ne se déphasent pas avec le temps. Qu'ils oscillent autour d'une vitesse n'est pas très grave, s'ils varient de vitesse en même temps. Comme çà on aura l'impression qu'ils tournent à une vitesse constante. 
 
Merci déjà pour tous les éclaircissements théoriques...  :blind:
 
Bonne journée.


#11 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 924 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 22 mars 2019 - 09:49

Suite du code : 

 

=> Augmentation de la résolution des pwm

=> Prise en compte du rapport de réduction et du nombre de tick pour afficher les valeurs en tours / min 
 

#include <DueTimer.h> // Pour gérer les interruptions sur Timer
#include <PID_v1.h>  // Pour le PID

/* 
 * Constantes Globales à modifier
 */

#define NBMOTEURS 5

// Pins des codeurs
const uint8_t PINENCODEURA[NBMOTEURS] = {32,34,36,38};
const uint8_t PINENCODEURB[NBMOTEURS] = {33,35,37,39};

// Timers
const uint16_t PERIODE = 50000; // période du Timer3 en µs qui exécute le calcul des vitesses
const uint32_t NBPERIODEPARMINUTE = 60000000 / PERIODE;
// Pins des moteurs 
const uint8_t PINVITESSEMOTEUR[NBMOTEURS] = {3,4,5,6,7};
const uint8_t PINSENSAMOTEUR[NBMOTEURS] = {22,24,26,28,30};
const uint8_t PINSENSBMOTEUR[NBMOTEURS] = {23,25,27,29,31};

// PID => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const double KP = 0.9, KI = 0.4, KD = 0; 
const uint16_t MAXPWM = 4095; // 4095 is the max for 12bits résolution
const uint8_t PWMRESOLUTION = 12; // With arduino due you can go to 12bits max résolution

// Constantes de conversions des unités => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const uint8_t NBTICKSPARTOURSCODEURS = 13;
const uint8_t GEARRATIO = 100;  
const uint16_t NBTICKSPARTOURSSORTIE =  NBTICKSPARTOURSCODEURS * GEARRATIO * 4;  // 4 car on prend sur A et sur B en front montant et décendant

// vitesseSortieRPM = vitesse * NBPERIODEPARMINUTE / NBTICKSPARTOURSSORTIE;
// consigneSortieTickParPeriode = consigneSortieRPM * NBTICKSPARTOURSSORTIE / NBPERIODEPARMINUTE;

/*
 * Variables globales 
 */

// Variables modifiées dans les interruptions => "volatile"
volatile int16_t compteur[NBMOTEURS] = {0,0,0,0,0};  // nombre de ticks compté
volatile int16_t vitesse[NBMOTEURS] = {0,0,0,0,0};    // nombre de ticks par unité de temps PERIODE3 
volatile bool dernierEtatEncodeurA[NBMOTEURS] = {0,0,0,0,0};
volatile bool dernierEtatEncodeurB[NBMOTEURS] = {0,0,0,0,0};
volatile bool flagTimer3 = 0;  // indique que l'interruption sur timer s'est effectuée

// Variables de PID ("double" car c'est la librairie utilisée qui veut ça ... )
double setpoint[NBMOTEURS] = {0,0,0,0,0};
double input[NBMOTEURS] = {0,0,0,0,0};
double output[NBMOTEURS] = {0,0,0,0,0};

// Tableau de pointeurs sur objets PID
PID PIDMOTEUR[NBMOTEURS] = {
  PID (&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT),
  PID (&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT),
  PID (&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT),
  PID (&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT),
  PID (&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT)
};

// Autre méthode d'écriture
/*PID PID0(&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT);
PID PID1(&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT);
PID PID2(&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT);
PID PID3(&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT);
PID PID4(&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT);

PID PIDMOTEURS[NBMOTEURS]= {PID0,PID1, PID2, PID3, PID4};*/


/* 
 * Fonctions  
 */


//Fonction de comptage des interruptions 

// Fonction de comptage générique 
void compteA( uint8_t i) {
  bool etatA = digitalRead(PINENCODEURA[i]);
  (etatA ^ dernierEtatEncodeurB[i]) ? compteur[i]++ :compteur[i]--;
  dernierEtatEncodeurA[i] = etatA;
}

void compteB( uint8_t i) {
  bool etatB = digitalRead(PINENCODEURB[i]);
  (etatB ^ dernierEtatEncodeurA[i]) ? compteur[i]-- : compteur[i]++;
  dernierEtatEncodeurB[i] = etatB;
}

// Fonctions d'interruption spécifique pour chaque pin codeur A et B
void compteA0() {
  compteA(0);
}
void compteA1() {
  compteA(1);
}
void compteA2() {
  compteA(2);
}
void compteA3() {
  compteA(3);
}
void compteA4() {
  compteA(4);
}

void compteB0() {
  compteB(0);
}
void compteB1() {
  compteB(1);
}
void compteB2() {
  compteB(2);
}
void compteB3() {
  compteB(3);
}
void compteB4() {
  compteB(4);
}

// Tableau de pointeurs sur fonction pour faciliter l'usage des interruptions
void (*compteEncodeurA[])(void)= {compteA0,compteA1,compteA2,compteA3,compteA4};
void (*compteEncodeurB[])(void)= {compteB0,compteB1,compteB2,compteB3,compteB4};

// Fonction timer
void timer3Interrupt() {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    vitesse[i] = compteur[i];
    compteur[i] = 0;
  }
  flagTimer3 = 1;
}

// fonction de pilotage des moteurs (à modifier en fonction du driver de moteur utilisé)
void setMoteur(uint8_t moteur, int16_t vitesse) {

  // Sécurités optionnelles pour s'assurer que les entrées sont bien dans les plages attendues...
  if(moteur >= NBMOTEURS) 
   return; 
  vitesse = constrain(vitesse,-MAXPWM, MAXPWM);  // Marges de PWM. 
  // Fin des sécurités optionnelles. 

  if(vitesse == 0 ) {
    // Frein d'urgence ou roue libre ... 
    // à configurer en fonction de ce que la carte moteur permet de faire..
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH);
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH);
  }
  if(vitesse > 0) {
    //Sens de contrôle indiqué comme positif par les codeurs
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH);
    digitalWrite(PINSENSBMOTEUR[moteur], LOW);
  } else {
    //Sens de contrôle indiqué comme négatif par les codeurs
    digitalWrite(PINSENSAMOTEUR[moteur], LOW);
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH);
  }
  analogWrite(PINVITESSEMOTEUR[moteur], abs(vitesse)); // écriture du PWM 
}

// Fonction de test qui fait tourner tous les moteur dans le même sens avec le même PWM
void testMoteurs( int16_t vitesse) {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    setMoteur(i, vitesse);
  }
}

// Fonctions asservissement PID 

void asservissementMoteur(uint8_t moteur) {
  input[moteur] = vitesse[moteur];
  PIDMOTEUR[moteur].Compute();
  setMoteur(moteur, (int)output[moteur]);
}

void asservissementMoteurs() {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    asservissementMoteur(i);
  }
}

// Fonction de conversion des unités 

double vitesseSortieRPM(uint8_t moteur) {
  return vitesse[moteur] * (double)NBPERIODEPARMINUTE / (double)NBTICKSPARTOURSSORTIE;
}

void setConsigneMoteur(uint8_t moteur, float rpm) {
  setpoint[moteur] = (int)(rpm * NBTICKSPARTOURSSORTIE / NBPERIODEPARMINUTE);
}

void setup() {

  Serial.begin(115200); 
  
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    
   // Initialisation des pins moteurs
   pinMode(PINVITESSEMOTEUR[i], OUTPUT);
   digitalWrite(PINVITESSEMOTEUR[i], LOW);
   pinMode(PINSENSAMOTEUR[i], OUTPUT); 
   digitalWrite(PINSENSAMOTEUR[i], LOW);
   pinMode(PINSENSBMOTEUR[i], OUTPUT);
   digitalWrite(PINSENSBMOTEUR[i], LOW);

   // Initialisation des codeurs
   pinMode(PINENCODEURA[i], INPUT_PULLUP);
   pinMode(PINENCODEURB[i], INPUT_PULLUP);
   attachInterrupt(digitalPinToInterrupt(PINENCODEURA[i]), compteEncodeurA[i], CHANGE);
   attachInterrupt(digitalPinToInterrupt(PINENCODEURB[i]), compteEncodeurB[i], CHANGE);
 
   // Initialisation des PID 
   PIDMOTEUR[i].SetMode(AUTOMATIC);
   analogWriteResolution(PWMRESOLUTION);
   PIDMOTEUR[i].SetOutputLimits(-MAXPWM, MAXPWM);

  }

  // Initialisation du timer
  Timer3.attachInterrupt(timer3Interrupt);
  Timer3.start(PERIODE); 

  
  //testMoteurs(127);

  // Initialisation des setpoints pour test d'asservissement
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
      setConsigneMoteur(i, 2.5);
  }
  
}

void loop() {
  
  if(flagTimer3) {
    asservissementMoteurs();     
    flagTimer3 = 0;

    // Affichage utile uniquement pour debugage
    for(uint8_t i = 0; i < NBMOTEURS; i++) {
      Serial.print(vitesseSortieRPM(i));
      Serial.print(",");    
    }
    Serial.println("");
  }
  
}


Par contre si on veut faire tourner un moteur doucement ou qu'on a une trop faible résolution codeur, on peut chercher à mesurer la vitesse en comptant le temps passé entre deux ticks au lieu de compter le nombre de ticks  pour un laps de temps donné.. 

Dans ce cas là il faudrait changer la méthode d'acquisition de la vitesse ... 

Par contre si le but est d'avoir aucun déphasage sur des hélices ... Un asservissement en position avec consigne  variable serait je pense plus adapté ... 


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#12 bvking

bvking

    Membre occasionnel

  • Membres
  • Pip
  • 98 messages

Posté 25 mars 2019 - 12:04

Hello,

 

J'ai filmé la rotation des hélices avec l'avant dernier et le dernier programme.

Avec pour chacun, un test PWM à 50 et un timer à 20000 car c'est comme ça que l'asservissement est le plus stable.

 

Il y a des decalages de phases.

 

J'ai essayé le dernier programme  avec un timer de 50000 et un PWM de 100 (pour avoir une vitesse réelle quasi équivalente au réglage testé précédemment)

et le déphasage à l'air moindre, mais loin d'être parfait...

 

Donc l'asservissement en position avec consigne variable sera le bienvenu. 8)

 

Merci .

Fichier(s) joint(s)



#13 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 924 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 25 mars 2019 - 02:05

Code devant permettre de réduire le déphase : 

#include <DueTimer.h> // Pour gérer les interruptions sur Timer
#include <PID_v1.h>  // Pour le PID

/* 
 * Constantes Globales à modifier
 */

#define NBMOTEURS 5

// Pins des codeurs
const uint8_t PINENCODEURA[NBMOTEURS] = {32,34,36,38};
const uint8_t PINENCODEURB[NBMOTEURS] = {33,35,37,39};

// Timers
const uint16_t PERIODE = 50000; // période du Timer3 en µs qui exécute le calcul des vitesses
const uint32_t NBPERIODEPARMINUTE = 60000000 / PERIODE;
// Pins des moteurs 
const uint8_t PINVITESSEMOTEUR[NBMOTEURS] = {3,4,5,6,7};
const uint8_t PINSENSAMOTEUR[NBMOTEURS] = {22,24,26,28,30};
const uint8_t PINSENSBMOTEUR[NBMOTEURS] = {23,25,27,29,31};

// PID => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const double KP = 0.9, KI = 0.4, KD = 0; 
const uint16_t MAXPWM = 4095; // 4095 is the max for 12bits résolution
const uint8_t PWMRESOLUTION = 12; // With arduino due you can go to 12bits max résolution

// Constantes de conversions des unités => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const uint8_t NBTICKSPARTOURSCODEURS = 13;
const uint8_t GEARRATIO = 100;  
const uint16_t NBTICKSPARTOURSSORTIE =  NBTICKSPARTOURSCODEURS * GEARRATIO * 4;  // 4 car on prend sur A et sur B en front montant et décendant

// vitesseSortieRPM = vitesse * NBPERIODEPARMINUTE / NBTICKSPARTOURSSORTIE;
// consigneSortieTickParPeriode = consigneSortieRPM * NBTICKSPARTOURSSORTIE / NBPERIODEPARMINUTE;

/*
 * Variables globales 
 */

// Variables modifiées dans les interruptions => "volatile"
volatile int16_t compteur[NBMOTEURS] = {0,0,0,0,0};  // nombre de ticks compté
volatile int32_t erreur[NBMOTEURS] = {0,0,0,0,0}; // conserve l'erreur
volatile uint16_t position[NBMOTEURS] = {0,0,0,0,0}; //  Position absolue comprise entre 0 et NBTICKSPARTOURSSORTIE
volatile int32_t vitesse[NBMOTEURS] = {0,0,0,0,0};    // nombre de ticks par unité de temps PERIODE3 
volatile bool dernierEtatEncodeurA[NBMOTEURS] = {0,0,0,0,0};
volatile bool dernierEtatEncodeurB[NBMOTEURS] = {0,0,0,0,0};
volatile bool flagTimer3 = 0;  // indique que l'interruption sur timer s'est effectuée

// Variables de PID ("double" car c'est la librairie utilisée qui veut ça ... )
double setpoint[NBMOTEURS] = {0,0,0,0,0};
double input[NBMOTEURS] = {0,0,0,0,0};
double output[NBMOTEURS] = {0,0,0,0,0};

// Tableau de pointeurs sur objets PID
PID PIDMOTEUR[NBMOTEURS] = {
  PID (&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT),
  PID (&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT),
  PID (&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT),
  PID (&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT),
  PID (&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT)
};

// Autre méthode d'écriture
/*PID PID0(&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT);
PID PID1(&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT);
PID PID2(&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT);
PID PID3(&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT);
PID PID4(&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT);

PID PIDMOTEURS[NBMOTEURS]= {PID0,PID1, PID2, PID3, PID4};*/


/* 
 * Fonctions  
 */


//Fonction de comptage des interruptions 

// Fonction de comptage générique 
void compteA( uint8_t i) {
  bool etatA = digitalRead(PINENCODEURA[i]);
  (etatA ^ dernierEtatEncodeurB[i]) ? compteur[i]++ :compteur[i]--;
  dernierEtatEncodeurA[i] = etatA;
}

void compteB( uint8_t i) {
  bool etatB = digitalRead(PINENCODEURB[i]);
  (etatB ^ dernierEtatEncodeurA[i]) ? compteur[i]-- : compteur[i]++;
  dernierEtatEncodeurB[i] = etatB;
}

// Fonctions d'interruption spécifique pour chaque pin codeur A et B
void compteA0() {
  compteA(0);
}
void compteA1() {
  compteA(1);
}
void compteA2() {
  compteA(2);
}
void compteA3() {
  compteA(3);
}
void compteA4() {
  compteA(4);
}

void compteB0() {
  compteB(0);
}
void compteB1() {
  compteB(1);
}
void compteB2() {
  compteB(2);
}
void compteB3() {
  compteB(3);
}
void compteB4() {
  compteB(4);
}

// Tableau de pointeurs sur fonction pour faciliter l'usage des interruptions
void (*compteEncodeurA[])(void)= {compteA0,compteA1,compteA2,compteA3,compteA4};
void (*compteEncodeurB[])(void)= {compteB0,compteB1,compteB2,compteB3,compteB4};

// Fonction timer
void timer3Interrupt() {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    vitesse[i] = compteur[i];
    compteur[i] = 0;
    position[i] += vitesse[i];
    while(position[i] >= NBTICKSPARTOURSSORTIE) {
      position[i] -= NBTICKSPARTOURSSORTIE; 
    }
    while(position[i] < 0) {
      position[i] += NBTICKSPARTOURSSORTIE; 
    }
    erreur[i] = vitesse[i] - (int)setpoint[i]; // conserve l'erreur au lieu de remettre à 0 ... => Transforme en asservissement en position
  } 
  flagTimer3 = 1; 
} 

// fonction de pilotage des moteurs (à modifier en fonction du driver de moteur utilisé) 
void setMoteur(uint8_t moteur, int16_t vitesse) { 
  
  // Sécurités optionnelles pour s'assurer que les entrées sont bien dans les plages attendues... 
  if(moteur >= NBMOTEURS) return; 
  vitesse = constrain(vitesse,-MAXPWM, MAXPWM); // Marges de PWM. 
  // Fin des sécurités optionnelles. 
  
  if(vitesse == 0 ) { 
    // Frein d'urgence ou roue libre ... 
    // à configurer en fonction de ce que la carte moteur permet de faire... 
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH); 
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH); 
  } 
  else if(vitesse > 0) { 
    //Sens de contrôle indiqué comme positif par les codeurs 
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH); 
    digitalWrite(PINSENSBMOTEUR[moteur], LOW); 
  } else { 
    //Sens de contrôle indiqué comme négatif par les codeurs 
    digitalWrite(PINSENSAMOTEUR[moteur], LOW); 
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH); 
  } 
  analogWrite(PINVITESSEMOTEUR[moteur], abs(vitesse)); // écriture du PWM 
} 

// Fonction de test qui fait tourner tous les moteur dans le même sens avec le même PWM 
void testMoteurs( int16_t vitesse) { 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    setMoteur(i, vitesse); 
  } 
}

// Fonctions asservissement PID 
void asservissementMoteur(uint8_t moteur) { 
  input[moteur] = vitesse[moteur] + erreur[moteur]; 
  PIDMOTEUR[moteur].Compute(); 
  setMoteur(moteur, (int)output[moteur]); 
} 

void asservissementMoteurs() { 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    asservissementMoteur(i); 
  } 
} 

// Fonction de conversion des unités 
double vitesseSortieRPM(uint8_t moteur) { 
  return vitesse[moteur] * (double)NBPERIODEPARMINUTE / (double)NBTICKSPARTOURSSORTIE; 
} 

void setConsigneMoteur(uint8_t moteur, double rpm) { 
  setpoint[moteur] = rpm * (double)NBTICKSPARTOURSSORTIE / (double)NBPERIODEPARMINUTE; 
} 

void setup() { 
  Serial.begin(115200); 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    // Initialisation des pins moteurs 
    pinMode(PINVITESSEMOTEUR[i], OUTPUT); 
    digitalWrite(PINVITESSEMOTEUR[i], LOW); 
    pinMode(PINSENSAMOTEUR[i], OUTPUT); 
    digitalWrite(PINSENSAMOTEUR[i], LOW); 
    pinMode(PINSENSBMOTEUR[i], OUTPUT); 
    digitalWrite(PINSENSBMOTEUR[i], LOW); 
    
    // Initialisation des codeurs 
    pinMode(PINENCODEURA[i], INPUT_PULLUP); 
    pinMode(PINENCODEURB[i], INPUT_PULLUP); 
    attachInterrupt(digitalPinToInterrupt(PINENCODEURA[i]), compteEncodeurA[i], CHANGE); 
    attachInterrupt(digitalPinToInterrupt(PINENCODEURB[i]), compteEncodeurB[i], CHANGE); 
    
    // Initialisation des PID 
    PIDMOTEUR[i].SetMode(AUTOMATIC); 
    analogWriteResolution(PWMRESOLUTION); 
    PIDMOTEUR[i].SetOutputLimits(-MAXPWM, MAXPWM); 
  } 
  
  // Initialisation du timer 
  Timer3.attachInterrupt(timer3Interrupt); 
  Timer3.start(PERIODE); 
  //testMoteurs(127); 
  
  // Initialisation des setpoints pour test d'asservissement 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    setConsigneMoteur(i, 2.5); 
  }   
} 

void loop() { 
  if(flagTimer3) { 
    asservissementMoteurs(); // Commenter cette ligne pour déplacer à la main le moteur 
    flagTimer3 = 0; 
    // Affichage utile uniquement pour debugage Serial.println("vitesseSortieRPM:"); 
    for(uint8_t i = 0; i < NBMOTEURS; i++) { 
      Serial.print(vitesseSortieRPM(i)); 
      Serial.print(", "); 
    } Serial.println(""); 
    Serial.println("position absolue en tick"); 
    for(uint8_t i = 0; i < NBMOTEURS; i++) { 
      Serial.print(position[i]); Serial.print(", "); 
    } 
    Serial.println(""); 
  } 
}
  


Par contre à partir de maintenant puisque tu as la fonction  " setConsigneMoteur(i, 2.5);
Tu peux directement imposer une vitesse en tour / min . 

( Et depuis le dernier code puisqu'on a augmenter la résolution des PWM le max pwm est désormais à 4095 pour 100% de vitesse. )


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#14 bvking

bvking

    Membre occasionnel

  • Membres
  • Pip
  • 98 messages

Posté 08 avril 2019 - 02:16

Récapitulatif:

1er programme : moteurs synchro.

2ème programme moteurs pas synchro.

 

Voici le code où les moteurs sont parfaitement synchro en phase quand la consigne moteur est à 50 tour/min.

Par contre, les moteurs se désynchronisent peu à peu quand la consigne moteur est à  25 tours/ min (soit 0,4 tours par seconde) ce qui j'imagine doit être normal, mais un petit peu dommage. 

 


#include <DueTimer.h> // Pour gérer les interruptions sur Timer
#include <PID_v1.h>  // Pour le PID

/* 
 * Constantes Globales à modifier
 */



#define NBMOTEURS 5

// Pins des codeurs
//const uint8_t PINENCODEURA[NBMOTEURS] =  {32,36,44,48};
//const uint8_t PINENCODEURB[NBMOTEURS] =  {33,37,45,49};

const uint8_t PINENCODEURA[NBMOTEURS] =  {32,52,44,48};
const uint8_t PINENCODEURB[NBMOTEURS] =  {33,53,45,49};


// Timers
const uint16_t PERIODE = 35000; // période du Timer3 en µs qui exécute le calcul des vitesses
const uint32_t NBPERIODEPARMINUTE = 60000000 / PERIODE;

// Pins des moteurs 
const uint8_t PINVITESSEMOTEUR[NBMOTEURS] = {3,4,5,6,7};
const uint8_t PINSENSAMOTEUR[NBMOTEURS] = {22,24,26,28};
const uint8_t PINSENSBMOTEUR[NBMOTEURS] = {23,25,27,29};

// PID => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const double KP = 1, KI = 1, KD = 0; // 0.9; 0,4
const uint16_t MAXPWM = 4095; // 4095 is the max for 12bits résolution
const uint8_t PWMRESOLUTION = 12; // With arduino due you can go to 12bits max résolution

// Constantes de conversions des unités => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const uint8_t NBTICKSPARTOURSCODEURS = 7;
const uint8_t GEARRATIO = 100;  
const uint16_t NBTICKSPARTOURSSORTIE =  NBTICKSPARTOURSCODEURS * GEARRATIO * 4;  // 4 car on prend sur A et sur B en front montant et décendant

//******************************************************************************************
//******************************************************************************************

// double vitesseSortieRPM = vitesse * NBPERIODEPARMINUTE / NBTICKSPARTOURSSORTIE;
// double consigneSortieTickParPeriode = consigneSortieRPM * NBTICKSPARTOURSSORTIE / NBPERIODEPARMINUTE;

/*
 * Variables globales 
 */

// Variables modifiées dans les interruptions => "volatile"
volatile int16_t compteur[NBMOTEURS] = {0,0,0,0,0};  // nombre de ticks compté
volatile int16_t vitesse[NBMOTEURS] = {0,0,0,0,0};    // nombre de ticks par unité de temps PERIODE3 
volatile bool dernierEtatEncodeurA[NBMOTEURS] = {0,0,0,0,0};
volatile bool dernierEtatEncodeurB[NBMOTEURS] = {0,0,0,0,0};
volatile bool flagTimer3 = 0;  // indique que l'interruption sur timer s'est effectuée

// Variables de PID ("double" car c'est la librairie utilisée qui veut ça ... )
double setpoint[NBMOTEURS] = {0,0,0,0,0};
double input[NBMOTEURS] = {0,0,0,0,0};
double output[NBMOTEURS] = {0,0,0,0,0};

// Tableau de pointeurs sur objets PID
PID PIDMOTEUR[NBMOTEURS] = {
  PID (&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT),
  PID (&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT),
  PID (&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT),
  PID (&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT),
  PID (&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT)
};

// Autre méthode d'écriture
/*PID PID0(&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT);
PID PID1(&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT);
PID PID2(&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT);
PID PID3(&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT);
PID PID4(&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT);

PID PIDMOTEURS[NBMOTEURS]= {PID0,PID1, PID2, PID3, PID4};*/


/* 
 * Fonctions  
 */


//Fonction de comptage des interruptions 

// Fonction de comptage générique 
void compteA( uint8_t i) {
  bool etatA = digitalRead(PINENCODEURA[i]);
  (etatA ^ dernierEtatEncodeurB[i]) ? compteur[i]++
:compteur[i]--;
  dernierEtatEncodeurA[i] = etatA;
}

void compteB( uint8_t i) {
  bool etatB = digitalRead(PINENCODEURB[i]);
  (etatB ^ dernierEtatEncodeurA[i]) ? compteur[i]-- : compteur[i]++;
  dernierEtatEncodeurB[i] = etatB;
}

// Fonctions d'interruption spécifique pour chaque pin codeur A et B
void compteA0() {
  compteA(0);
}
void compteA1() {
  compteA(1);
}
void compteA2() {
  compteA(2);
}
void compteA3() {
  compteA(3);
}
void compteA4() {
  compteA(4);
}

void compteB0() {
  compteB(0);
}
void compteB1() {
  compteB(1);
}
void compteB2() {
  compteB(2);
}
void compteB3() {
  compteB(3);
}
void compteB4() {
  compteB(4);
}

// Tableau de pointeurs sur fonction pour faciliter l'usage des interruptions
void (*compteEncodeurA[])(void)= {compteA0,compteA1,compteA2,compteA3,compteA4};
void (*compteEncodeurB[])(void)= {compteB0,compteB1,compteB2,compteB3,compteB4};

// Fonction timer
void timer3Interrupt() {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
   
vitesse[i] = compteur[i];
    compteur[i] -= (int)setpoint[i]; // conserve l'erreur au lieu de remettre à 0 ... => Transforme en asservissement en position 
  }
  flagTimer3 = 1;
}

// fonction de pilotage des moteurs (à modifier en fonction du driver de moteur utilisé)
void setMoteur(uint8_t moteur, int16_t vitesse) {

  // Sécurités optionnelles pour s'assurer que les entrées sont bien dans les plages attendues...
  if(moteur >= NBMOTEURS) 
   return; 
  vitesse = constrain(vitesse,-MAXPWM, MAXPWM);  // Marges de PWM. 
  // Fin des sécurités optionnelles. 

  if(vitesse == 0 ) {
    // Frein d'urgence ou roue libre ... 
    // à configurer en fonction de ce que la carte moteur permet de faire..
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH);
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH);
  }
  if(vitesse > 0) {
    //Sens de contrôle indiqué comme positif par les codeurs
   
digitalWrite(PINSENSAMOTEUR[moteur], HIGH);
    digitalWrite(PINSENSBMOTEUR[moteur], LOW);
  } else {
    //Sens de contrôle indiqué comme négatif par les codeurs
    digitalWrite(PINSENSAMOTEUR[moteur], LOW);
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH);
  }
  analogWrite(PINVITESSEMOTEUR[moteur], abs(vitesse)); // écriture du PWM 
}

// Fonction de test qui fait tourner tous les moteur dans le même sens avec le même PWM
void testMoteurs( int16_t vitesse) {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    setMoteur(i, vitesse);
  }
}

// Fonctions asservissement PID 

void asservissementMoteur(uint8_t moteur) {
  input[moteur] = vitesse[moteur];
  PIDMOTEUR[moteur].Compute();
  setMoteur(moteur, (int)output[moteur]);
}

void asservissementMoteurs() {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    asservissementMoteur(i);
  }
}

// Fonction de conversion des unités 

double vitesseSortieRPM(uint8_t moteur) {
  return vitesse[moteur] * (double)NBPERIODEPARMINUTE / (double)NBTICKSPARTOURSSORTIE;
}

void setConsigneMoteur(uint8_t moteur, double rpm) {
  setpoint[moteur] = rpm * (double)NBTICKSPARTOURSSORTIE / (double)NBPERIODEPARMINUTE;
}




void setup() {
  
  Serial.begin(115200); 
  
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    
   // Initialisation des pins moteurs
   pinMode(PINVITESSEMOTEUR[i], OUTPUT);
   digitalWrite(PINVITESSEMOTEUR[i], LOW);
   pinMode(PINSENSAMOTEUR[i], OUTPUT); 
   digitalWrite(PINSENSAMOTEUR[i], LOW);
   pinMode(PINSENSBMOTEUR[i], OUTPUT);
   digitalWrite(PINSENSBMOTEUR[i], LOW);

   // Initialisation des codeurs
   pinMode(PINENCODEURA[i], INPUT_PULLUP);
   pinMode(PINENCODEURB[i], INPUT_PULLUP);
   attachInterrupt(digitalPinToInterrupt(PINENCODEURA[i]), compteEncodeurA[i], CHANGE);
   attachInterrupt(digitalPinToInterrupt(PINENCODEURB[i]), compteEncodeurB[i], CHANGE);

   // Initialisation des PID 
   PIDMOTEUR[i].SetMode(AUTOMATIC);
   analogWriteResolution(PWMRESOLUTION);

 PIDMOTEUR[i].SetOutputLimits(-MAXPWM, MAXPWM);

  }

  // Initialisation du timer
  Timer3.attachInterrupt(timer3Interrupt);
  Timer3.start(PERIODE); 

  
  //testMoteurs(127);

  // Initialisation des setpoints pour test d'asservissement
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
      setConsigneMoteur(i, 25.26);
      setConsigneMoteur (4,0);
  }

}

void loop() {


    
  if(flagTimer3) {
    asservissementMoteurs();     
    flagTimer3 = 0;

   // Affichage utile uniquement pour debugage
    for(uint8_t i = 0; i < NBMOTEURS; i++) {
      Serial.print(vitesseSortieRPM(i));
      Serial.print(",");    
    }
    
    Serial.println("");
  }
 }

2 eme programme: pas de synchro

#include <DueTimer.h> // Pour gérer les interruptions sur Timer
#include <PID_v1.h>  // Pour le PID

/* 
 * Constantes Globales à modifier
 */

#define NBMOTEURS 5

// Pins des codeurs
const uint8_t PINENCODEURA[NBMOTEURS] =  {32,52,44,48};
const uint8_t PINENCODEURB[NBMOTEURS] =  {33,53,45,49};
// Timers
const uint16_t PERIODE = 35000; // période du Timer3 en µs qui exécute le calcul des vitesses
const uint32_t NBPERIODEPARMINUTE = 60000000 / PERIODE;
// Pins des moteurs 
const uint8_t PINVITESSEMOTEUR[NBMOTEURS] = {3,4,5,6,7};
const uint8_t PINSENSAMOTEUR[NBMOTEURS] = {22,24,26,28};
const uint8_t PINSENSBMOTEUR[NBMOTEURS] = {23,25,27,29};

// PID => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const double KP = 1, KI = 1, KD = 0; 
const uint16_t MAXPWM = 4095; // 4095 is the max for 12bits résolution
const uint8_t PWMRESOLUTION = 12; // With arduino due you can go to 12bits max résolution

// Constantes de conversions des unités => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const uint8_t NBTICKSPARTOURSCODEURS = 7;
const uint8_t GEARRATIO = 100;  
const uint16_t NBTICKSPARTOURSSORTIE =  NBTICKSPARTOURSCODEURS * GEARRATIO * 4;  // 4 car on prend sur A et sur B en front montant et décendant

// vitesseSortieRPM = vitesse * NBPERIODEPARMINUTE / NBTICKSPARTOURSSORTIE;
// consigneSortieTickParPeriode = consigneSortieRPM * NBTICKSPARTOURSSORTIE / NBPERIODEPARMINUTE;

/*
 * Variables globales 
 */

// Variables modifiées dans les interruptions => "volatile"
volatile int16_t compteur[NBMOTEURS] = {0,0,0,0,0};  // nombre de ticks compté
volatile int32_t erreur[NBMOTEURS] = {0,0,0,0,0}; // conserve l'erreur
volatile int16_t position[NBMOTEURS] = {0,0,0,0,0}; //  Position absolue comprise entre 0 et NBTICKSPARTOURSSORTIE
volatile int32_t vitesse[NBMOTEURS] = {0,0,0,0,0};    // nombre de ticks par unité de temps PERIODE3 
volatile bool dernierEtatEncodeurA[NBMOTEURS] = {0,0,0,0,0};
volatile bool dernierEtatEncodeurB[NBMOTEURS] = {0,0,0,0,0};
volatile bool flagTimer3 = 0;  // indique que l'interruption sur timer s'est effectuée

// Variables de PID ("double" car c'est la librairie utilisée qui veut ça ... )
double setpoint[NBMOTEURS] = {0,0,0,0,0};
double input[NBMOTEURS] = {0,0,0,0,0};
double output[NBMOTEURS] = {0,0,0,0,0};

// Tableau de pointeurs sur objets PID
PID PIDMOTEUR[NBMOTEURS] = {
  PID (&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT),
  PID (&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT),
  PID (&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT),
  PID (&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT),
  PID (&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT)
};

// Autre méthode d'écriture
/*PID PID0(&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT);
PID PID1(&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT);
PID PID2(&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT);
PID PID3(&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT);
PID PID4(&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT);

PID PIDMOTEURS[NBMOTEURS]= {PID0,PID1, PID2, PID3, PID4};*/


/* 
 * Fonctions  
 */


//Fonction de comptage des interruptions 

// Fonction de comptage générique 
void compteA( uint8_t i) {
  bool etatA = digitalRead(PINENCODEURA[i]);
  (etatA ^ dernierEtatEncodeurB[i]) ? compteur[i]++ :compteur[i]--;
  dernierEtatEncodeurA[i] = etatA;
}

void compteB( uint8_t i) {
  bool etatB = digitalRead(PINENCODEURB[i]);
  (etatB ^ dernierEtatEncodeurA[i]) ? compteur[i]-- : compteur[i]++;
  dernierEtatEncodeurB[i] = etatB;
}

// Fonctions d'interruption spécifique pour chaque pin codeur A et B
void compteA0() {
  compteA(0);
}
void compteA1() {
  compteA(1);
}
void compteA2() {
  compteA(2);
}
void compteA3() {
  compteA(3);
}
void compteA4() {
  compteA(4);
}

void compteB0() {
  compteB(0);
}
void compteB1() {
  compteB(1);
}
void compteB2() {
  compteB(2);
}
void compteB3() {
  compteB(3);
}
void compteB4() {
  compteB(4);
}

// Tableau de pointeurs sur fonction pour faciliter l'usage des interruptions
void (*compteEncodeurA[])(void)= {compteA0,compteA1,compteA2,compteA3,compteA4};
void (*compteEncodeurB[])(void)= {compteB0,compteB1,compteB2,compteB3,compteB4};

// Fonction timer
void timer3Interrupt() {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    vitesse[i] = compteur[i];
    compteur[i] = 0;
    position[i] += vitesse[i];
    
    while(position[i] >= NBTICKSPARTOURSSORTIE) {
      position[i] -= NBTICKSPARTOURSSORTIE; 
    }
    while(position[i] < 0) { // pourquoi ZERO, pourquoi pas NBTICKSPARTOURSSORTIE?
      position[i] += NBTICKSPARTOURSSORTIE; 
    }
    erreur[i] += vitesse[i] - (int)setpoint[i]; // conserve l'erreur au lieu de remettre à 0 ... => Transforme en asservissement en position
  } 
  flagTimer3 = 1; 
} 

// fonction de pilotage des moteurs (à modifier en fonction du driver de moteur utilisé) 
void setMoteur(uint8_t moteur, int16_t vitesse) { 
  
  // Sécurités optionnelles pour s'assurer que les entrées sont bien dans les plages attendues... 
  if(moteur >= NBMOTEURS) return; 
  vitesse = constrain(vitesse,-MAXPWM, MAXPWM); // Marges de PWM. 
  // Fin des sécurités optionnelles. 
  
  if(vitesse == 0 ) { 
    // Frein d'urgence ou roue libre ... 
    // à configurer en fonction de ce que la carte moteur permet de faire... 
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH); 
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH); 
  } 
  else if(vitesse > 0) { 
    //Sens de contrôle indiqué comme positif par les codeurs 
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH); 
    digitalWrite(PINSENSBMOTEUR[moteur], LOW); 
  } else { 
    //Sens de contrôle indiqué comme négatif par les codeurs 
    digitalWrite(PINSENSAMOTEUR[moteur], LOW); 
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH); 
  } 
  analogWrite(PINVITESSEMOTEUR[moteur], abs(vitesse)); // écriture du PWM 
} 

// Fonction de test qui fait tourner tous les moteur dans le même sens avec le même PWM 
void testMoteurs( int16_t vitesse) { 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    setMoteur(i, vitesse); 
  } 
}

// Fonctions asservissement PID 
void asservissementMoteur(uint8_t moteur) { 
  input[moteur] = vitesse[moteur] + erreur[moteur]; 
  PIDMOTEUR[moteur].Compute(); 
  setMoteur(moteur, (int)output[moteur]); 
} 

void asservissementMoteurs() { 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    asservissementMoteur(i); 
  } 
} 

// Fonction de conversion des unités 
double vitesseSortieRPM(uint8_t moteur) { 
  return vitesse[moteur] * (double)NBPERIODEPARMINUTE / (double)NBTICKSPARTOURSSORTIE; 
} 

void setConsigneMoteur(uint8_t moteur, double rpm) { 
  setpoint[moteur] = rpm * (double)NBTICKSPARTOURSSORTIE / (double)NBPERIODEPARMINUTE; 
} 

void setup() { 
  Serial.begin(115200); 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    // Initialisation des pins moteurs 
    pinMode(PINVITESSEMOTEUR[i], OUTPUT); 
    digitalWrite(PINVITESSEMOTEUR[i], LOW); 
    pinMode(PINSENSAMOTEUR[i], OUTPUT); 
    digitalWrite(PINSENSAMOTEUR[i], LOW); 
    pinMode(PINSENSBMOTEUR[i], OUTPUT); 
    digitalWrite(PINSENSBMOTEUR[i], LOW); 
    
    // Initialisation des codeurs 
    pinMode(PINENCODEURA[i], INPUT_PULLUP); 
    pinMode(PINENCODEURB[i], INPUT_PULLUP); 
    attachInterrupt(digitalPinToInterrupt(PINENCODEURA[i]), compteEncodeurA[i], CHANGE); 
    attachInterrupt(digitalPinToInterrupt(PINENCODEURB[i]), compteEncodeurB[i], CHANGE); 
    
    // Initialisation des PID 
    PIDMOTEUR[i].SetMode(AUTOMATIC); 
    analogWriteResolution(PWMRESOLUTION); 
    PIDMOTEUR[i].SetOutputLimits(-MAXPWM, MAXPWM); 
  } 
  
  // Initialisation du timer 
  Timer3.attachInterrupt(timer3Interrupt); 
  Timer3.start(PERIODE); 
  //testMoteurs(127); 
  
  // Initialisation des setpoints pour test d'asservissement 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    setConsigneMoteur(i, 50.10); 
  }   
} 

void loop() { 
  if(flagTimer3) { 
    asservissementMoteurs(); // Commenter cette ligne pour déplacer à la main le moteur 
    flagTimer3 = 0; 
    // Affichage utile uniquement pour debugage Serial.println("vitesseSortieRPM:"); 
    for(uint8_t i = 0; i < NBMOTEURS; i++) { 
      Serial.print(vitesseSortieRPM(i)); 
      Serial.print(", "); 
    } Serial.println(""); 
    Serial.println("position absolue en tick"); 
    for(uint8_t i = 0; i < NBMOTEURS; i++) { 
      Serial.print(position[i]); Serial.print(", "); 
    } 
    Serial.println(""); 
  } 
}


#15 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 924 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 10 avril 2019 - 10:19

J'ai directement modifié le deuxième code 

Il y avait une petite erreur : 

 

erreur[i] = vitesse[i] - (int)setpoint[i];

 

a été remplacé par : 

erreur[i] += vitesse[i] - (int)setpoint[i];

Le problème devrait maintenant être corrigé =)


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#16 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 9 924 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 25 juin 2019 - 03:30

Exemple de code qui je pense peut permettre de faire un truc assez sympas : 

 

deux fonctions codées yoyo1 et yoyo2 : 

 

#include <DueTimer.h> // Pour gérer les interruptions sur Timer
#include <PID_v1.h>  // Pour le PID

/* 
 * Constantes Globales à modifier
 */

#define NBMOTEURS 5

// Pins des codeurs
const uint8_t PINENCODEURA[NBMOTEURS] =  {32,52,44,48};
const uint8_t PINENCODEURB[NBMOTEURS] =  {33,53,45,49};
// Timers
const uint16_t PERIODE = 35000; // période du Timer3 en µs qui exécute le calcul des vitesses
const uint32_t NBPERIODEPARMINUTE = 60000000 / PERIODE;
// Pins des moteurs 
const uint8_t PINVITESSEMOTEUR[NBMOTEURS] = {3,4,5,6,7};
const uint8_t PINSENSAMOTEUR[NBMOTEURS] = {22,24,26,28};
const uint8_t PINSENSBMOTEUR[NBMOTEURS] = {23,25,27,29};

// PID => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const double KP = 1, KI = 1, KD = 0; 
const uint16_t MAXPWM = 4095; // 4095 is the max for 12bits résolution
const uint8_t PWMRESOLUTION = 12; // With arduino due you can go to 12bits max résolution

// Constantes de conversions des unités => Possibilité de les mettre sous forme de tableaux si il y a plusieurs moteurs différents
const uint8_t NBTICKSPARTOURSCODEURS = 7;
const uint8_t GEARRATIO = 100;  
const uint16_t NBTICKSPARTOURSSORTIE =  NBTICKSPARTOURSCODEURS * GEARRATIO * 4;  // 4 car on prend sur A et sur B en front montant et décendant

// Modes de fonctionnement 

const uint8_t POSITION = 0;
const uint8_t VITESSE = 1;
const uint8_t SUIVRE0 = 2;
const uint8_t SUIVRE4 = 3;
const uint8_t RECALLAGE0 = 4;
const uint8_t RECALLAGE4 = 5;

// vitesseSortieRPM = vitesse * NBPERIODEPARMINUTE / NBTICKSPARTOURSSORTIE;
// consigneSortieTickParPeriode = consigneSortieRPM * NBTICKSPARTOURSSORTIE / NBPERIODEPARMINUTE;

/*
 * Variables globales 
 */

// Variables modifiées dans les interruptions => "volatile"
volatile int16_t compteur[NBMOTEURS] = {0,0,0,0,0};  // nombre de ticks compté
volatile int32_t erreur[NBMOTEURS] = {0,0,0,0,0}; // conserve l'erreur
volatile int16_t position[NBMOTEURS] = {0,0,0,0,0}; //  Position absolue comprise entre 0 et NBTICKSPARTOURSSORTIE
volatile int32_t vitesse[NBMOTEURS] = {0,0,0,0,0};    // nombre de ticks par unité de temps PERIODE3 
volatile bool dernierEtatEncodeurA[NBMOTEURS] = {0,0,0,0,0};
volatile bool dernierEtatEncodeurB[NBMOTEURS] = {0,0,0,0,0};
volatile bool flagTimer3 = 0;  // indique que l'interruption sur timer s'est effectuée

// Variables de PID ("double" car c'est la librairie utilisée qui veut ça ... )
double setpoint[NBMOTEURS] = {0,0,0,0,0};
double input[NBMOTEURS] = {0,0,0,0,0};
double output[NBMOTEURS] = {0,0,0,0,0};

// Tableau de pointeurs sur objets PID
PID PIDMOTEUR[NBMOTEURS] = {
  PID (&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT),
  PID (&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT),
  PID (&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT),
  PID (&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT),
  PID (&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT)
};

// Autre méthode d'écriture
/*PID PID0(&input[0], &output[0], &setpoint[0], KP, KI, KD, DIRECT);
PID PID1(&input[1], &output[1], &setpoint[1], KP, KI, KD, DIRECT);
PID PID2(&input[2], &output[2], &setpoint[2], KP, KI, KD, DIRECT);
PID PID3(&input[3], &output[3], &setpoint[3], KP, KI, KD, DIRECT);
PID PID4(&input[4], &output[4], &setpoint[4], KP, KI, KD, DIRECT);

PID PIDMOTEURS[NBMOTEURS]= {PID0,PID1, PID2, PID3, PID4};*/


/* 
 * Fonctions  
 */


//Fonction de comptage des interruptions 

// Fonction de comptage générique 
void compteA( uint8_t i) {
  bool etatA = digitalRead(PINENCODEURA[i]);
  (etatA ^ dernierEtatEncodeurB[i]) ? compteur[i]++ :compteur[i]--;
  dernierEtatEncodeurA[i] = etatA;
}

void compteB( uint8_t i) {
  bool etatB = digitalRead(PINENCODEURB[i]);
  (etatB ^ dernierEtatEncodeurA[i]) ? compteur[i]-- : compteur[i]++;
  dernierEtatEncodeurB[i] = etatB;
}

// Fonctions d'interruption spécifique pour chaque pin codeur A et B
void compteA0() {
  compteA(0);
}
void compteA1() {
  compteA(1);
}
void compteA2() {
  compteA(2);
}
void compteA3() {
  compteA(3);
}
void compteA4() {
  compteA(4);
}

void compteB0() {
  compteB(0);
}
void compteB1() {
  compteB(1);
}
void compteB2() {
  compteB(2);
}
void compteB3() {
  compteB(3);
}
void compteB4() {
  compteB(4);
}

// Tableau de pointeurs sur fonction pour faciliter l'usage des interruptions
void (*compteEncodeurA[])(void)= {compteA0,compteA1,compteA2,compteA3,compteA4};
void (*compteEncodeurB[])(void)= {compteB0,compteB1,compteB2,compteB3,compteB4};

uint8_t mode = POSITION;

// Fonction timer
void timer3Interrupt() {
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    vitesse[i] = compteur[i];
    compteur[i] = 0;
    position[i] += vitesse[i];
    
    while(position[i] >= NBTICKSPARTOURSSORTIE) {
      position[i] -= NBTICKSPARTOURSSORTIE; 
    }
    while(position[i] < 0) { 
      position[i] += NBTICKSPARTOURSSORTIE; 
    }
  }

  // Calculs d'erreur séparé car corrélation entre les 5 moteurs
  for(uint8_t i = 0; i < NBMOTEURS; i++) {
    switch (mode) {
     case POSITION:
      erreur[i] += vitesse[i] - (int)setpoint[i]; // conserve l'erreur au lieu de remettre à 0 ... => Transforme en asservissement en "mouvement" : concerve la synchro
    break;
     case VITESSE:
      erreur[i] = vitesse[i] - (int)setpoint[i];   // concerve la vitesse sans la synchro
    break;
     case SUIVRE0:
      if(i != 0 ) {
       erreur[i] = vitesse[i] - vitesse[0] + position[i] - position[0];   // concerve la vitesse sans la synchro
       if (erreur[i] > NBTICKSPARTOURSSORTIE /2) 
        erreur[i] -= NBTICKSPARTOURSSORTIE;
       if (erreur[i] < -NBTICKSPARTOURSSORTIE /2) 
        erreur[i] += NBTICKSPARTOURSSORTIE;
      } else 
       erreur[i] = vitesse[i] - (int)setpoint[i];
    break;
     case SUIVRE4:
      if(i != 4 ) {
       erreur[i] = vitesse[i] - vitesse[4] + position[i] - position[4];   // concerve la vitesse sans la synchro
       if (erreur[i] > NBTICKSPARTOURSSORTIE /2) 
        erreur[i] -= NBTICKSPARTOURSSORTIE;
       if (erreur[i] < -NBTICKSPARTOURSSORTIE /2) 
        erreur[i] += NBTICKSPARTOURSSORTIE;
      } else 
       erreur[i] = vitesse[i] - (int)setpoint[i];
    break;
     case RECALLAGE0:
      if(i != 0 ) {
       erreur[i] =  vitesse[i] - vitesse[i - 1] + position[i] - position[i-1];   // concerve la vitesse sans la synchro
       if (erreur[i] > NBTICKSPARTOURSSORTIE /2) 
        erreur[i] -= NBTICKSPARTOURSSORTIE;
       if (erreur[i] < -NBTICKSPARTOURSSORTIE /2) 
        erreur[i] += NBTICKSPARTOURSSORTIE;
      } else 
       erreur[i] = vitesse[i] - (int)setpoint[i]; 
    break;
     case RECALLAGE4:
      if(i != 4 ) {
       erreur[i] = vitesse[i] - vitesse[i + 1] + position[i] - position[i+1];   // concerve la vitesse sans la synchro
       if (erreur[i] > NBTICKSPARTOURSSORTIE /2) 
        erreur[i] -= NBTICKSPARTOURSSORTIE;
       if (erreur[i] < -NBTICKSPARTOURSSORTIE /2) 
        erreur[i] += NBTICKSPARTOURSSORTIE;
      } else 
       erreur[i] = vitesse[i] - (int)setpoint[i];
     break;
    }
  } 
  flagTimer3 = 1; 
} 

// fonction de pilotage des moteurs (à modifier en fonction du driver de moteur utilisé) 
void setMoteur(uint8_t moteur, int16_t vitesse) { 
  
  // Sécurités optionnelles pour s'assurer que les entrées sont bien dans les plages attendues... 
  if(moteur >= NBMOTEURS) return; 
  vitesse = constrain(vitesse,-MAXPWM, MAXPWM); // Marges de PWM. 
  // Fin des sécurités optionnelles. 
  
  if(vitesse == 0 ) { 
    // Frein d'urgence ou roue libre ... 
    // à configurer en fonction de ce que la carte moteur permet de faire... 
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH); 
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH); 
  } 
  else if(vitesse > 0) { 
    //Sens de contrôle indiqué comme positif par les codeurs 
    digitalWrite(PINSENSAMOTEUR[moteur], HIGH); 
    digitalWrite(PINSENSBMOTEUR[moteur], LOW); 
  } else { 
    //Sens de contrôle indiqué comme négatif par les codeurs 
    digitalWrite(PINSENSAMOTEUR[moteur], LOW); 
    digitalWrite(PINSENSBMOTEUR[moteur], HIGH); 
  } 
  analogWrite(PINVITESSEMOTEUR[moteur], abs(vitesse)); // écriture du PWM 
} 

// Fonction de test qui fait tourner tous les moteur dans le même sens avec le même PWM 
void testMoteurs( int16_t vitesse) { 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    setMoteur(i, vitesse); 
  } 
}

// Fonctions asservissement PID 
void asservissementMoteur(uint8_t moteur) { 
  input[moteur] = vitesse[moteur] + erreur[moteur]; 
  PIDMOTEUR[moteur].Compute(); 
  setMoteur(moteur, (int)output[moteur]); 
} 

void asservissementMoteurs() { 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    asservissementMoteur(i); 
  } 
} 

// Fonction de conversion des unités 
double vitesseSortieRPM(uint8_t moteur) { 
  return vitesse[moteur] * (double)NBPERIODEPARMINUTE / (double)NBTICKSPARTOURSSORTIE; 
} 

int32_t vitesseRPMEnTick(double rpm) {
  return (int32_t)(rpm * (double)NBTICKSPARTOURSSORTIE / (double)NBPERIODEPARMINUTE);
}
void setConsigneMoteur(uint8_t moteur, double rpm) { 
  setpoint[moteur] = vitesseRPMEnTick(rpm); 
} 

void setup() { 
  Serial.begin(115200); 
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    // Initialisation des pins moteurs 
    pinMode(PINVITESSEMOTEUR[i], OUTPUT); 
    digitalWrite(PINVITESSEMOTEUR[i], LOW); 
    pinMode(PINSENSAMOTEUR[i], OUTPUT); 
    digitalWrite(PINSENSAMOTEUR[i], LOW); 
    pinMode(PINSENSBMOTEUR[i], OUTPUT); 
    digitalWrite(PINSENSBMOTEUR[i], LOW); 
    
    // Initialisation des codeurs 
    pinMode(PINENCODEURA[i], INPUT_PULLUP); 
    pinMode(PINENCODEURB[i], INPUT_PULLUP); 
    attachInterrupt(digitalPinToInterrupt(PINENCODEURA[i]), compteEncodeurA[i], CHANGE); 
    attachInterrupt(digitalPinToInterrupt(PINENCODEURB[i]), compteEncodeurB[i], CHANGE); 
    
    // Initialisation des PID 
    PIDMOTEUR[i].SetMode(AUTOMATIC); 
    PIDMOTEUR[i].SetSampleTime(PERIODE / 100);
    analogWriteResolution(PWMRESOLUTION); 
    PIDMOTEUR[i].SetOutputLimits(-MAXPWM, MAXPWM); 
  } 
  
  // Initialisation du timer 
  Timer3.attachInterrupt(timer3Interrupt); 
  Timer3.start(PERIODE); 
  
  //testMoteurs(127); 
  
  // Initialisation des setpoints pour test d'asservissement 
  /*
  for(uint8_t i = 0; i < NBMOTEURS; i++) { 
    setConsigneMoteur(i, 50.10); 
  }   
  */
} 

void yoyo1(double rpm, int16_t temps) {   // temps en secondes
  static uint8_t etat = 0;
  static uint32_t reftemps = 0;
  
   switch (etat) {
     case 0: 
      if(millis() - reftemps > temps * 1000 / 4 ) {  // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = POSITION;
       for(uint8_t i = 0; i < NBMOTEURS; i++) { 
        setConsigneMoteur(i, (rpm * temps  + (i - 2)* 60) / (float)temps); 
       } 
       reftemps = millis();
       etat = 1;
      }
    break;
     case 1:
      if(millis() - reftemps > temps * 1000 / 4 ) { // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = POSITION;
       for(uint8_t i = 0; i < NBMOTEURS; i++) { 
        setConsigneMoteur(i, (rpm * temps  +(i+2)*60) / (float)temps); 
       }
       reftemps = millis();
       etat = 0; 
      }
    break;
   }
}

void yoyo2(double rpm, int16_t temps) {   // temps en secondes
  static uint8_t etat = 0;
  static uint32_t reftemps = 0;
  
   switch (etat) {
     case 0: 
      if(millis() - reftemps > temps * 1000 / 4 ) {  // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = POSITION;
       for(uint8_t i = 0; i < NBMOTEURS; i++) { 
        setConsigneMoteur(i, (rpm * temps  + (i - 4)* 60) / (float)temps); 
       } 
       reftemps = millis();
       etat = 1;
      }
    break;
     case 1:
      if(millis() - reftemps > temps * 1000 / 4 ) { // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = RECALLAGE4;
       reftemps = millis();
       etat = 2; 
      }
    break;
     case 2: 
      if(millis() - reftemps > temps * 1000 / 4 ) {  // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = POSITION;
       for(uint8_t i = 0; i < NBMOTEURS; i++) { 
        setConsigneMoteur(i, (rpm * temps  + i * 60) / (float)temps); 
       } 
       reftemps = millis();
       etat = 3;
      }
    break;
     case 3:
      if(millis() - reftemps > temps * 1000 / 4 ) { // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = RECALLAGE0;
       reftemps = millis();
       etat = 0; 
      }
   }
}


void loop() { 
  yoyo1(50, 120);   // 50 vitesse moyenne en RPM 120, temps en secondes par cycles
  //yoyo2(50, 120);
  asservissementMoteurs(); // Commenter cette ligne pour déplacer à la main le moteur 
  if(flagTimer3) { 
    flagTimer3 = 0; 
    // Affichage utile uniquement pour debugage 
    /*
    Serial.print("Mode: "); 
    Serial.println(mode); 
    */

    /*
    Serial.println("vitesseSortieRPM:"); 
    for(uint8_t i = 0; i < NBMOTEURS; i++) { 
      Serial.print(vitesseSortieRPM(i)); 
      Serial.print(", "); 
    } Serial.println(""); 

    */

    /*
    Serial.println("PositionEnTicks"); 
    for(uint8_t i = 0; i < NBMOTEURS; i++) { 
      Serial.print(position[i]); 
      Serial.print(", "); 
    } Serial.println(""); 

    */
    
    //Serial.println(""); 
  } 
}

Dis moi si ça fait l'effet attendu ! =)


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 
Si vous souhaitez un robot pilotable par internet n'hésitez pas à visiter www.vigibot.com et à lire le sous forum dédié à vigibot!

 

Les réalisations de Mike118  

 

 

 


#17 bvking

bvking

    Membre occasionnel

  • Membres
  • Pip
  • 98 messages

Posté 27 juin 2019 - 03:12

Super! Motif yoyo1 concluant!

 

Après avoir divisé par cinq (car j'ai cinq moteurs) et nom par quatre le temps des cycles par lequel l'écart entre les moteurs s'incrémente et se decremente voici ce que j'obtiens en video.

Pour avoir un joli mouvement hélicoïdal, j'ai aussi augmenté le  paramètre i-2 à i-10

setConsigneMoteur(i, (rpm * temps + (i - 10)* 60) / (float)temps); //2 mieux=10

 

Merci.

Je teste l'autre motif demain

 switch (etat) {
     case 0: 
      if(millis() - reftemps > temps * 1000 / 5 ) {  // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = POSITION;
       for(uint8_t i = 0; i < 5; i++) { 
        setConsigneMoteur(i, (rpm * temps  + (i - 10)* 60) / (float)temps); //2 mieux=10
       } 
       reftemps = millis();
       etat = 1;
       Serial.println ("etat1");
      }
    break;
     case 1:
      if(millis() - reftemps > temps * 1000 / 5 ) { // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = POSITION;
       for(uint8_t i = 0; i < 5; i++) { 
        setConsigneMoteur(i, (rpm * temps  +(i+ 10)*60) / (float)temps); 
       }
       reftemps = millis();
       etat = 0; 
        Serial.println ("etat0");
      }
    break;

Fichier(s) joint(s)



#18 bvking

bvking

    Membre occasionnel

  • Membres
  • Pip
  • 98 messages

Posté 28 juin 2019 - 02:26

Voici l'effet de yoyo2.

J'ai changé des réglages.

On voit bien le moteur du fond se déphaser vers l'avant (dans le sens de rotation), puis les autres suivent et le mouvement est joli.

Puis arriver à un certain écart ( déphasage entre les moteurs), on voit un redressement et  pendant un cours instant,

le moteur de devant se déphase vers l'avant (il rattrape son retard) et les autres suivent pour en fin se re-synchroniser en phase.

Puis on revient au départ.

Comment faire pour que après le redressement ce soit le moteur de devant qui soit la reference et non celui du fond?

Dans le case2 à (i +2.5), j'ai essayé de mettre un moins et même juste i, mais ça change rien j'ai aucun changement de motif.

Je comprends pas trop.

Merci, en tous cas, ça s'annonce pas mal!  :thank_you:

void yoyo2(double rpm, int16_t temps) {   // temps en secondes
  static uint8_t etat = 0;
  static uint32_t reftemps = 0;
  
   switch (etat) {
     case 0: 
      if(millis() - reftemps > temps * 1000 / 15) {//4 probleme helice (premiere et derniere vont trop vite) 15 ça va // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = POSITION;
       for(uint8_t i = 0; i < NBMOTEURS; i++) { 
        setConsigneMoteur(i, (rpm * temps  + (i - 2.5)* 60) / (float)temps); // 2.5, 5 OK car multiple de 5 ???;// 4; 20 --> la derniere helice tourne trop vite et saccade; pareil avec autre helice
       } 
       reftemps = millis();
       etat = 1;
      }
    break;
     case 1:
      if(millis() - reftemps > temps * 1000 / 10) { // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = RECALLAGE4;
       reftemps = millis();
       etat = 2; 
      }
    break;
     case 2: 
     
      if(millis() - reftemps > temps * 1000 / 15) {  // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = POSITION;
       for(uint8_t i = 0; i < NBMOTEURS; i++) { 
        setConsigneMoteur(i, (rpm * temps  + (i +2.5) * 60) / (float)temps); // tu avais rien mis à (i +2.5), seulement i, c'est exprès?
       } 
       reftemps = millis();
       etat = 3;
      }
    break;
     case 3:
      if(millis() - reftemps > temps * 1000 / 10) { // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = RECALLAGE0;
       reftemps = millis();
       etat = 0; 
      }
   }
}

Fichier(s) joint(s)



#19 bvking

bvking

    Membre occasionnel

  • Membres
  • Pip
  • 98 messages

Posté 19 septembre 2019 - 02:28

Hello Jo, 

Pour avoir un mouvement plus fluide à l'oeil nu, j'ai pris des moteurs allant jusqu'à 30 tr/min avec ces caractéristiques:

 

L'extrémité du moteur du codeur 11 signale

 

Tension d'entrée (V)

12

Ratio de réduction de retardateur

1: 200

Pas de vitesse de charge (RPM)

30

Pas de courant de charge (mA)

≤60

Couple nominal (kg.cm)

10.0 Kg.cm

Vitesse nominale (RPM)

21

Courant nominal (A)

≤0.4

décrochage Torque

≥ 32 Kg.cm

StallCurrent (A)

≤1.0

Résolution Hall

2200

 

J'ai réglé le timer à 5000 pour m'approcher d'une vitesse de 127 avec un PWM de 127.

 

Les 5 moteurs sont parfaitement synchronisés ( même vitesse, pas de décalage de phase).

 

J'ai juste changé la vitesse de rotation dans void loop() {

yoyo1(50, 120); en

yoyo1(20, 120);

 

et dans le setup () { la ligne:

if(millis() - reftemps > temps * 1000 / 4 en

if(millis() - reftemps > temps * 1000 / 5  car j'ai cinq moteurs et çà fonctionnait mieux comme çà avec l'autre configuration

 

 

 

void yoyo1(double rpm, int16_t temps) {   // temps en secondes

  static uint8_t etat = 0;
  static uint32_t reftemps = 0;
  
   switch (etat) {
     case 0: 
      if(millis() - reftemps > temps * 1000 / 5 ) {// 5-->15 accelere les cycles de repetions  // 1000 car comparaison de secondes avec ms et 4 car quart de tour  
       mode = POSITION;
       for(uint8_t i = 0; i < 5; i++) { 
        setConsigneMoteur(i, (rpm * temps  + (i-2 )* 60) / (float)temps); // 5(2 bizarre--> 5, 10  l'ecart inter phase est plus lissé (moins grand)
       } 
       reftemps = millis();
       
       Serial.println (" **********************             etat1?"); 
       Serial.println (" **********************             etat1?"); 
       Serial.println (" **********************             etat1?"); 
       Serial.println (" **********************             etat1?"); 
       Serial.println (" **********************             etat1?"); 
       Serial.println (" **********************             etat1?"); 
       Serial.println (etat+5);
       
       etat = 1;
      
      }
    break;
     case 1:
      if(millis() - reftemps > temps * 1000 / 5) { // 1000 car comparaison de secondes avec ms et 4 car quart de tour
       mode = POSITION;
       for(uint8_t i = 0; i < 5; i++) { 
        setConsigneMoteur(i, (rpm * temps  +(i+2 )*60) / (float)temps); //(0-->5, 10 ok 15= pas de vitesse qui accelere ou decelèrer entre etat
       }
       reftemps = millis();
       
       Serial.println ("  ********************              etat0?"); 
       Serial.println ("  ********************              etat0?");  
       Serial.println ("  ********************              etat0?"); 
       Serial.println ("  ********************              etat0?"); 
       Serial.println ("  ********************              etat0?"); 
       Serial.println ("  ********************              etat0?"); 
       Serial.println ("  ********************              etat0?"); 
       Serial.println ("  ********************              etat0?"); 
       Serial.println (etat+5);
    
       etat = 0; 
    
      }
    break;
   }
}
 
Avec l'autre config, les moteurs de 6 volt à 90 tour/min, le motif fonctionnait avec 50 tr/ min et ne fonctionnait plus avec 40 tours/min.
 
J'ai fait une video en piece jointe.
 
Et j'ai mis le programme aussi.

 

 

Merci pour ton regard aiguisé.

 

 

 

Fichier(s) joint(s)



#20 bvking

bvking

    Membre occasionnel

  • Membres
  • Pip
  • 98 messages

Posté 20 septembre 2019 - 05:47

Re-salut!

 

Je mets une video qui sera peut être plus explicite.

 

Le  motif fonctionne pendant à peu près deux minutes, puis l'hélice du centre, la troisieme sur cinq, la i=2, se décale par rapport aux autres et finie par se synchroniser sur la i=3.

 

Alors que l'écart devrait être constant du debut à la fin et tourner en boucle.

 

J'imagine qu'ensuite, la 3 se cale sur la 4, puis la 4 sur 0 et ainsi de suite. Mais je suppose seulement.

 

Je vérifirais.

 

Je me disais ensuite qu'il serait peut être interessant, que je choisisse qu'elle se décale les unes par rapport aux autres, à chaque fois qu'un cycle ait été fait.

 

Merci. :Koshechka_08:

 

 

Fichier(s) joint(s)







Aussi étiqueté avec au moins un de ces mots-clés : Arduino Due, Codeurs, PID, Asservissement

0 utilisateur(s) li(sen)t ce sujet

0 members, 0 guests, 0 anonymous users