¶ Asservissement PID
Les 2 cavaliers EN_A et EN_B doivent être retirés.
Le cavalier 5VEN, alimentation 5V pour la carte Arduino, doit être positionné.
// asservissement d'un moteur CC en vitesse avec un correcteur PID.
#include <SimpleTimer.h>
SimpleTimer timer; // Timer pour échantillonnage
const boolean reponseEchelon = false;
unsigned int tick_codeuse = 0; // Compteur de tick de la codeuse
int vitMoteur = 0; // Commande du moteur
const int frequence_echantillonnage = 100; // Fréquence d'exécution de l'asservissement
const float rapport_reducteur = 48; // Rapport nombre de tours de l'arbre moteur et de la roue
const int tick_par_tour_codeuse = 80;
// unsigned int time = 0;
//definition des entrées
const int pinInput1 = 8; // Commande de sens moteur, Input 1
const int pinInput2 = 9; // Commande de sens moteur, Input 2
const int pinPower = 5; // Commande de vitesse moteur, Output Enabled1
//consigne en tour/s
float consigne_moteur = 1; // Consigne nombre de tours de roue par seconde
// init calculs asservissement PID
float erreur_precedente = consigne_moteur; // (en tour/s)
float somme_erreur = 0;
//Definition des constantes du correcteur PID
const float kp = 4276; // Coefficient proportionnel (choisis par essais successifs)
const float ki = 0.033; // Coefficient intégrateur
const float kd = 0.016; // Coefficient dérivateur
//P : Kp=7800
//PI : Kp=7000, Ki=0.067
//PID: Kp=9300, Ki=0.040, Kd=0.010
/* Routine d'initialisation */
void setup() {
Serial.begin(115200); // Initialisation port COM
pinMode(pinPower, OUTPUT);
pinMode( pinInput1, OUTPUT );
pinMode( pinInput2, OUTPUT );
digitalWrite(pinInput1, LOW);
digitalWrite(pinInput2, HIGH);
analogWrite(pinPower, 0);
plot(0, 0);
// Pause de 0,5 sec pour laisser le temps au moteur de s'arréter si celui-ci est en marche et marqué le début de l'échelon
delay(500);
plot(0, 0);
// Interruption sur tick de la codeuse (interruption 0 = pin2 arduino)
attachInterrupt(0, compteur, CHANGE); // Pin 2
attachInterrupt(1, compteur, CHANGE); // Pin 3
// Interruption pour calcul du PID et asservissement appelee toutes les 10ms
timer.setInterval(1000 / frequence_echantillonnage, asservissement);
}
/* Fonction principale */
void loop() {
timer.run(); //on fait tourner l'horloge
delay(10);
}
//---- Interruption sur tick de la codeuse
void compteur() {
tick_codeuse++;
}
//---- Interruption pour calcul du P
void asservissement() {
// Calcul de l'erreur
int frequence_codeuse = frequence_echantillonnage * tick_codeuse; //100*tick_codeuse
float vit_roue_tour_sec = (float)frequence_codeuse / (float)tick_par_tour_codeuse / (float)rapport_reducteur;
float erreur = consigne_moteur - vit_roue_tour_sec;
somme_erreur += erreur;
float delta_erreur = erreur - erreur_precedente;
erreur_precedente = erreur;
// P : calcul de la commande
vitMoteur = kp*erreur + ki*somme_erreur + kd*delta_erreur; //somme des trois erreurs
// Normalisation et contrôle du moteur
if ( vitMoteur > 255 ) {
vitMoteur = 255; // Max du pont en H L293D
} else if ( vitMoteur < 0 ) {
vitMoteur = 0;
}
if(reponseEchelon) {
analogWrite(pinPower, 255);
plot(3, vit_roue_tour_sec);
} else {
analogWrite(pinPower, vitMoteur);
plot(consigne_moteur, vit_roue_tour_sec);
}
// Réinitialisation du nombre de tick de la codeuse
tick_codeuse = 0;
}
void plot(float cons, float value) {
Serial.print(cons);
Serial.print("\t");
Serial.print(value, 8);
Serial.println();
}