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("");
}
}
