Après m'être fait la main sur un asservissement en vitesse d'un moteur, je m'attaque à l'asservissement polaire. J'aurai ainsi une consigne de distance et une consigne d'angle qui prend en compte l'ensemble des 2 moteurs.
Au début j'ai lu pas mal de pages internet sur le sujet, j'ai griffoné pas mal de schémas bloc, je me suis demandé comment j'allais traduire ça en code et puis, petit à petit, je suis arrivé à un truc qui semble tenir la route.
Il n'y a plus que deux points que je n'arrive pas à comprendre et dont je n'ai pas trouvé les réponses en cherchant moi même. C'est la raison de ce post.
Premier point:
Dans mon asservissement en vitesse je commence par mesurer la vitesse de rotation de la roue grâce aux nombre de "tick" compté par les encodeurs. Les "tick" sont ensuite remis à 0 pour l'échantillon suivant.
Pour l'exemple, un extrait de ma fonction d'asser en vitesse:
// affectation des compteurs de PID et reset des compteurs sur interruption compteurD = tickD; tickD = 0; mesure_vit_d = (freq_ech*compteurD)/tick_trm/reduction; // vitesse mesurée en tr/s
Maintenant je part du principe que dans un asservissement polaire, le but étant de parcourir une certaine distance (par exemple 1000 tick), je ne suis pas censé remettre à 0 le compteur de tick n'est-ce pas ?
Second point:
Contrairement à un asser en vitesse, la fonction d'asservissement polaire doit gérer le sens de rotation des moteurs (si j'ai bien compris). C'est là que je bloque car je vois pas comment faire ça.
Je poste ici la fonction d'asservissement que j'ai réalisé jusque là:
//--------------------------------------------------------------------------- // INTERRUPTIONS EXTERNES - //--------------------------------------------------------------------------- ISR(INT0_vect) { // Le sens de rotation des moteur est décodé en hardware et sert à incrémenter ou décrémenter les compteur de tick sur interruption. // si EncodDSens = 0 (on avance) alors incrémenter TickD sinon décrémenter if ( bit_is_clear(EncodDSens_portE, EncodDSens_broche)) { tickD++; } else { tickD--; } } ISR(INT1_vect) { // si EncodGSens = 0 (on avance) alors décrémenter TickG sinon incrémenter (car cet encodeur est inversé) if ( bit_is_clear(EncodGSens_portE, EncodGSens_broche)) { tickG--; } else { tickG++; } } //--------------------------------------------------------------------------- // FONCTION ASSERVISSEMENT PID Polaire - //--------------------------------------------------------------------------- void pid(float ConsigneDist, float ConsigneOrient) { float compteurD; float compteurG; float mesure_dist; float mesure_orient; float cmd_dist; float cmd_orient; // affectation des compteurs de PID et reset des compteurs sur interruption //compteurD = tickD; //compteurG = tickG; //tickD = 0; //tickG = 0; // mesure distance et orientation mesure_dist = (tickD + tickG)/2; mesure_orient = tickD - tickG; // Calcul des erreurs de distance erreur_dist = ConsigneDist - mesure_dist; somme_erreur_dist += erreur_dist; delta_erreur_dist = erreur_dist - erreur_prec_dist; // mise à jour de l'erreur précédente erreur_prec_dist = erreur_dist; // Calcul des erreurs d'orientation erreur_orient = ConsigneOrient - mesure_orient; somme_erreur_orient += erreur_orient; delta_erreur_orient = erreur_orient - erreur_prec_orient; // mise à jour de l'erreur précédente erreur_prec_orient = erreur_orient; // calcul des commandes cmd_dist = ((kp_dist * erreur_dist) + (ki_dist * somme_erreur_dist) + (kd_dist * delta_erreur_dist)); // PID distance cmd_orient = ((kp_orient * erreur_orient) + (ki_orient * somme_erreur_orient) + (kd_orient * delta_erreur_orient)); // PID orientation // appliquer les commandes aux moteur PWMG = cmd_dist - cmd_orient; // ça ne doit surement pas fonctionner de cette façon PWMD = cmd_dist + cmd_orient; // Normalisation des commandes PWM de sortie (le moteur ne bouge pas avec un pwm < 240) if (PWMD < 240) {PWMD = 240;} else if (PWMD > 1023) {PWMD = 1023;} if (PWMG < 240) {PWMG = 240;} else if (PWMG > 1023) {PWMG = 1023;} }
et la fonction principale:
int main (void) { Ports_Init(); // initialisations E/S Init(); // initialisation avant démarrage Consigne_Dist = 0; Consigne_Orient = 0; somme_erreur_dist = 0; somme_erreur_orient = 0; erreur_prec_dist = Consigne_Dist; // initialisation sur consigne de depart erreur_prec_orient = Consigne_Orient; _delay_ms(1000); Consigne_Dist = 18000; // 18000 ticks = 1 tour de roue while (1) { pid(Consigne_Dist, Consigne_Orient); _delay_ms(20); // PID à 50Hz (F=1/t) } return 1; }
En vous remerciant !