/********************************************************************/ // Contrôle commande de moteur brushless "aviation" // // Mesure par capteur infrarouge Sharp sur AN0. // // Ajout d'un correcteur PID à la régulation proportionnelle // /********************************************************************/ //Contrôle du niveau de la réserve d'air par //potentiomètre sur AN1, seulement en phase d'initialisation // Le potentiomètre de gain est sur AN2 . /********************************************************************/ // Méthode de réglage à l'initialisation, si nécessaire : // Si elle a déjà été faite dans le programme CommandeNo2, // que l'on a utilisé pour mesurer la période de l'oscillation de "pompage", // il n'est pas nécessaire de la refaire. // Sinon : /****************************************************************************************/ /********* Mettre D6 et D4 à 0 à l'aide d'un shunt pour se mettre en phase de réglage.*********/ /****************************************************************************************/ // Mettre le potentiomètre de gain en position médiane et le potentiomètre // de niveau tourné du côté OV. // Mettre en marche et ajuster le potentiomètre de niveau pour que // la réserve d'air se gonfle à la hauteur désirée, sans ou très peu // de fuite d'air. Lors de cette phase le disposistif se comporte // exactement de la même façon que lors du test du programme nommé : // CommandeNo0. On accélère uniquement le moteur, il n'y a pas de régulation. // Quand la réserve d'air est à la hauteur désirée : // Retirer le shunt : les valeurs sont enregistrée en EEPROM. // Attendre que le programme fasse une phase de démarrage progressif puis // ajuster le gain afin d'avoir une régulation energique, sans atteindre // le valeur qui provoque le "pompage" de la réserve d'air. // // Le potentiomètre de niveau est devenu inactif en fonctionnement normal. // /***********************************/ #include // pour lire ou stocker les valeurs correspondantes à la hauteur désirée // et la puissance qui lui est associée /************************************/ //Mesure par capteur infrarouge Sharp sur AN0. /************************************/ //Préréglage du niveau de la réserve d'air par //potentiomètre sur AN1 // Le potentiomètre de gain est sur AN2 . /************************************/ // Le maintien dr D4 à O volt correspond à la régulation proportionnelle // comme le programme de CommandeNo2 // ****************Correcteur PID************************** // // Pour faire une correction PID // le coefficient Kp du correcteur est laissé à 1, le potentiomètre GAIN joue ce rôle. // Deux grandeurs sont prises pour calculer le correcteur : // Tu, la période de l'oscillation en limite de contre-réaction proportionnelle // Te, la période d'échantillonnage. // ************************ Entrer les bonnes valeurs ******************* // const float Tu = 0.92; //seconde const float Te = 0.020; //seconde // ********************************************************** // // Déclarations de variables globales int distance=0; int offset_puiss= 0; // un décalage de niveau de puissance // enregistré en EEPROM au cours du réglage préliminaire int hauteur_base=0; // hauteur de la réserve d'air, enregistrée // en EEPROM au cours du réglage préliminaire int gain = 0; // gain ajustable par potentiomètre int dist_utile=0;// variable associée au capteur ou au potentiomètre unsigned int temps_pulse; // largeur d'impulsion de la sortie int sortie_servo=9; // numéro des PIN du module arduino utilisé en sortie pour la connexion // à la commande du contrôleur brushless . unsigned long debut_periode; // pour appliquer la période de 20 millisecondes qui doit séparer les // impulsions de commande du contrôleur brushless. int pinDistance =0,pinOffset=1,pinGain=2; // numéro de PIN de conversion analogique digitale du module arduino // utilisé pour lire la tension du capteur ou des potentiomètres. int pinReglage=6, pinBasculePID=4; // n° de pin à mettre à zéro pour utiliser le réglage préliminaire // n° de pin à mettre à 1 pour passer du mode proportionnel au mode PID boolean flag_demarre=1,flag_enregistre=0; // Pour déterminer la durée que l'on laisse au contrôleur du moteur // pour exécuter son initialisation interne. // flag_enregistre est utilisé uniquement avec le réglage préliminaire. long tempo_dist,tempo_gain; // utilisées parfois pour éviter des débordements dans des multiplications. // Grandeur d'entrée et ses 2 valeurs retardées, pour le calcul PID numérique. float en[3]= {200.0}; // valeur initiale arbitraire // Grandeur de sortie et ses 2 valeurs retardées. float yn[3] = {200.0}; // valeur initiale arbitraire // Coefficients du correcteur numérique d'ordre 2 float r0,r1,r2,s1,s2; // Méthode proposée dans le livre de Jean Marie RETIF // "Synthèse d'une commande robuste", Editions Ellipses. // Discrétisation des correcteurs p 139. void calcul_parametrePID(float Tu,float Te) { float sii,Ti,Td,Nd; Nd = 12.0; Ti = 0.5 * Tu; Td = 0.125 * Tu; sii = -( Td / (Td + Nd*Te) ); r0 = (1 + Te/Ti - Nd*sii) ; r1 = sii*(1 + Te/Ti + 2*Nd) -1; r2 = -sii*(1 + Nd); s1 = -(1 - sii); s2 = -sii; } void correcteurPID(int dist_utile) { en[0] = float (dist_utile); // Calcul yn[0] = r0*en[0] + r1*en[1] + r2*en[2] - s1*yn[1] - s2*yn[2]; // éviter une trop forte saturation de l'intégrateur if (yn[0] > 1024.0-offset_puiss) yn[0] =1024.0-offset_puiss; if (yn[0] < -offset_puiss) yn[0] = -offset_puiss; // Transfert pour le calcul suivant en[2] = en[1]; en[1] = en[0]; yn[2] = yn[1]; yn[1] = yn[0]; // yn[0] n'est pas modifié par le transfert // sa valeur est directement utilisable pour la fonction génératrice du pulse } void mesures(void) // fontion appelée pour mesurer l'entrée analogique { distance=0; gain = 0; //la valeur de distance est moyennée for(int ii=0; ii<16; ii++) { distance += analogRead(pinDistance); gain += analogRead(pinGain); } //distance est prise une 16 fois . //et divisé par 16 (en décalant 4 fois à droite) pour faire une moyenne // afin de limiter les effets de certains relevés décalés par des parasites. dist_utile = (distance >>4); // est divisé par 16 pour revenir entre 0 et 1023 dist_utile -= hauteur_base; // en plus ou moins sur la valeur de base // correspond à l'erreur de régulation gain >>= 4; // est divisé par 16 pour revenir entre 0 et 1023 /**********************************************************************/ // L'effet du potentiomètre de gain sera de multiplier la valeur de l'erreur // représentée par dist_utile, d'un coefficient qui varie linéairement de // 1/4 lorsque gain = 0 à 1 lorsque gain vaut 512. // Dans la plage de valeur supérieure, l'erreur représentée par dist_utile // sera multipliée par un coefficient qui varie linéairement de 1 pour // gain = 512 à 4 pour gain = 1024. // En option, une autre formule pour que ce facteur multiplicatif gain // varie de 1 à 8 en plage haute du potentiomètre, ou de 1 à 16 , // ou de 1/8 à 1 en plage inférieure. // L'utilisation d'un entier long pour ce calcul permet de garder une certaine // précision dans le calcul en évitant les débordements possibles avec une // variable de type int. /**********************************************************************/ tempo_dist = long(dist_utile); tempo_gain = long(gain); if(gain < 513) { // formule pour que le gain varie entre 1/4 et 1 tempo_dist = (tempo_dist * (512 + (3*tempo_gain))); tempo_dist >>= 11; // équivalent à la division par 4*512 /* // formule pour que le gain varie entre 1/8 et 1 tempo_dist = (tempo_dist * (512 + (7*tempo_gain))); tempo_dist >>= 12; // équivalent à la division par 8*512 */ } if (gain > 512) { // formule pour que que le gain varie de 1 à 4 tempo_dist = (tempo_dist * ( (3 * gain) - 1024) ); tempo_dist >>= 9; // équivalent à la division par 512 /* // formule pour que que le gain varie de 1 à 8 tempo_dist = (tempo_dist * ( (7 * gain) - 3072) ); tempo_dist >>= 9; // équivalent à la division par 512 */ /* // formule pour que que le gain varie de 1 à 16 tempo_dist = (tempo_dist * ( (15 * gain) - 7168) ); tempo_dist >>= 9; // équivalent à la division par 512 */ } dist_utile = int(tempo_dist); /*********************************************************************/ //utiliser la ligne suivante si la tension augmente quand la réserve //d'air augmente, sinon il faut la mettre en // commentaire. /*********************************************************************/ /*********************************************************************/ dist_utile = - dist_utile; /*********************************************************************/ /*********************************************************************/ // Serial.print("Distance utile :"); // Serial.println(dist_utile); // Affichage éventuel pour vérifier la mesure } void faire_pulse(int commande) { temps_pulse = (990 + constrain(commande,0,1023)); // constrain utilisé pour que la durée de pulse reste dans les // limites de environ 1 à 2 msec, même en cas d'errer sur commande. digitalWrite(sortie_servo,HIGH); delayMicroseconds(temps_pulse); digitalWrite(sortie_servo,LOW); } void setup()//Initialise l'utilisation des PINs de l'arduino { pinMode(sortie_servo,OUTPUT); digitalWrite(sortie_servo,LOW); pinMode(pinReglage,INPUT); // Une résistance de tirage au +Vcc . pinMode(pinBasculePID,INPUT); // Une résistance de tirage au +Vcc . Serial.begin(9600);// pour déboggage éventuel analogReference(DEFAULT);// la référence du convertisseur // est la tension d'alimentation calcul_parametrePID(Tu,Te);// Les valeurs de Tu et Te définies au départ // Pour voir le résultat du calcul Serial.print("r0 = "); Serial.println(r0,6); Serial.print("r1 = "); Serial.println(r1,6); Serial.print("r2 = "); Serial.println(r2,6); Serial.print("s1 = "); Serial.println(s1,6); Serial.print("s2 = "); Serial.println(s2,6); Serial.flush(); // Si l'on veut, fonction d'attente pour lire les résultats // avant que le programme ne se poursuive. /* int attend =1; while(attend) { if(Serial.available() >0) { attend = Serial.read();//vide le buffer attend = 0; // finir la boucle d'attente } } */ } unsigned compte=0; // variable utilisée pour le comptage dans des boucles void loop() { // Si nécessaire, programmation en EEPROM du niveau de base de la // réserve d'air, et de la valeur de la commande correspondante. unsigned int compte_reglage=0; while( !digitalRead(pinReglage) ) // teste si on demande un nouveau réglage des consignes de régulation // Si oui : // Le potentiomètre de niveau règle la puissance moteur, // L'entrée A0 est lue par le convertisseur Analogique-Numérique // Aucune régulation n'est effectuée. // Quand le shunt sur D6 est ôté, le niveau de puissance moteur // et la valeur de A0 sont enregistrés en EEPROM pour servir // de consigne à la boucle de régulation. { distance=0; gain = 0; //la valeur de distance est moyennée debut_periode = micros(); for(int ii=0; ii<16; ii++) { distance += analogRead(pinDistance); offset_puiss += analogRead(pinOffset); } hauteur_base = (distance >>4); offset_puiss >>= 4; // pour revenir aux valeurs moyenne flag_enregistre = 1; faire_pulse(offset_puiss); if(compte_reglage>150)// peut visualiser toutes les 3 secondes // le niveau des réglages { compte_reglage = 0; Serial.print("hauteur_base = "); Serial.println(hauteur_base); Serial.print("offset_puiss = "); Serial.println(offset_puiss); } compte_reglage++; while( (micros()-debut_periode) < 19990); // attendre les 20 millisecondes pour la période des pulses } // Si la boucle de réglage a été demandée, enregistrer les consignes à sa sortie : if(flag_enregistre) { // enregistrer en EEPROM les réglages EEPROM.write(0,highByte(hauteur_base)); EEPROM.write(1,lowByte(hauteur_base)); EEPROM.write(2,highByte(offset_puiss)); EEPROM.write(3,lowByte(offset_puiss)); flag_enregistre=0; } // Sinon : // lire les réglages enregistrés en EEPROM dans un réglage précédent offset_puiss= (EEPROM.read(2)<<8)+EEPROM.read(3); hauteur_base = (EEPROM.read(0)<<8)+EEPROM.read(1); //affichage des valeurs lues pour débugger, si nécessaire. Serial.print("hauteur_base = "); Serial.println(hauteur_base); Serial.print("offset_puiss = "); Serial.println(offset_puiss); delay(1000); // 6 secondes pour que le contrôleur brushless s'initialise // il émet habituellement un signal sonore à ce moment. while(flag_demarre) { debut_periode= micros();// la période durera 20 millisecondes faire_pulse(0); // l'impulsion doit être à sa valeur minimale // pendant toute la durée où le contrôleur du moteur fair // son initialisation. compte++; if(compte>300) // (300 x 20 msec = 6 sec) { compte = 0; //reset de compte pour une réutilisation possible flag_demarre = 0; // pour finir la boucle } while( (micros()-debut_periode) < 19990); // attendre les 20 millisecondes pour la période des pulses } //refaire une boucle pour une augmentation progressive de la puissance //afin de ne pas bousculer trop rapidement la réserve d'air. //On augmente la largeur d'impulsion de 1 microseconde toutes les 20 millisecondes //jusqu'à ce que la valeur maximale soit atteinte, ou que le niveau de signal //délivré par le capteur de hauteur de la réserve d'air soit atteint. flag_demarre = 1; while(flag_demarre) { debut_periode = micros(); faire_pulse( compte ); compte++; if(compte > 1023) flag_demarre=0;//puissance max atteinte mesures(); if(compte > dist_utile+offset_puiss) flag_demarre = 0; // consigne atteinte if(compte < 100) flag_demarre= 1; //empêche la sortie prématurée de boucle pendant le 2 premières //secondes, laissant les effets de moyenne se stabiliser while( (micros()-debut_periode) < 19990){}; // attendre les 20 millisecondes pour la période des impulsions } // Passage en fonctionnement normal : boucle infinie while(1) { debut_periode = micros(); mesures(); correcteurPID(dist_utile); // yn[0] est le résultat du traitement de dist_utile, c'est à dire, la valeur // de l'erreur de régulation, traitée par le correcteur PID. if (digitalRead(pinBasculePID)) faire_pulse( int(yn[0]) + offset_puiss ); else faire_pulse(dist_utile + offset_puiss ); while( (micros()-debut_periode) < 19990){}; // attendre les 20 millisecondes pour la période des impulsions } }