<< The Fribotte Homepage >>
Un club de passionnés en robotique participant à la coupe de France E=M6.
[Accueil] [Qui sommes-nous ?] [Robots] [Coupe e=m6] [BD Technique] [Forum] [Reportages] [Liens] [WiKiFri]

Fribotte


Généralités - Les contraintes de temps
Le temps, c'est important

On va surtout étudier ici le problème majeur au niveau de la gestion du temps, qui est le problème du compatage des impulsions venant des 2 encodeurs. On a ici 34µs entre 2 fronts montants sur un encodeur et il y a 2 encodeurs.

Il faut non seulement compter les impulsions mais aussi déterminer le sens de celles-ci.

Rappel : le 16F877 à 20Mhz exécute une instruction toutes les 200 ns.

8.5µs -> 42 instructions sur le 16F877.

Note : Il y a un autre moyen très simple de déterminer le sens de rotation. Il suffit en effet d'utiliser une simple bascule D par encodeur. Mais voulant ici n'utiliser que le 16F877 sans composant externe, cette tâche sera réalisée logicellement.

 

La base de temps


La base de temps est celle utilisée pour le PID et l'échantillonnage.
Elle sert de référence pour l'échantillonnage des compteurs, et c'est à la fin de la base de temps que le calcul du PID est effectué, avec à la clef deux nouveaux PWM.
Trop rapide elle ne permet pas d'avoir suffisamment d'impulsions comptées pour la précision du calcul.
Trop lente, elle ne permet pas de bien réguler l'asservissement efficacement.

La solution retenue ici est d'avoir une base de temps réglable par l'utilisateur, en multiple de ms (10ms pour Nikoro).

La base de temps sera de 4ms au minimum.

Attention que les coeficients du PID (en particulier le Ki) évoluent avec la base temps. Il peut être donc délicat de la changer souvent.


Les grandes lignes du programme
Description générale


Le 16F877 a 3 timers.
Le timer 2 est utilisé pour la génération des 2 PWMs (voir plus loin), ainsi que pour la détermination de la base de temps.

Le timer 0, qui est sur 8 bits et le timer 1, sur 16 bits, est utilisé comme compteurs sur les signaux des encodeurs des 2 moteurs.
(Une interruption sur le timer 0 permetrait de le gérer en 16 bits, comme le timer 1, mais ce n'est pas nécessaire).

Le programme principal prend a forme d'une boucle qui durera le temps défini par la base de temps (en utilisant une interruption sur le timer 2 pour avoir précisément la fin de la base de temps).

Dans cette boucle le programme a surtout pour tache de tester le sens de rotation des 2 encodeurs (voir ci-desous) tout en sommant les impulsions des encodeurs.
Suivant le sens de rotation mesuré, la différence entre l'état précédent et l'état actuel des compteurs est ajoutée ou retranchée à des variables contenant l'avancement total des encodeurs.

Le programme vérifie aussi si de nouvelles consignes lui sont parvenues.

A la fin de la boucle, le programme calcule le PID, et donne les nouvelles valeurs aux PWMs.
Il applique ensuite les nouvelles consignes (changement de base de temps, etc ...). Il peut retourner l'état des compteurs au µc/µp maître (il les garde en mémoire de toute façon).

L'idée principale du programme est que pendant le calcul du PID, il n'y a pas détermination du sens. Le 16F877 continue de compter les impulsions avec les 2 compteurs hardwares. Mais il ne détermine plus le sens logiciellement. En fait on part du principe que le sens ne se met a changer que si le moteur ralentit (ce qui est normal, il ne va pas passer en 100µs de +1m/s à-1m/s).

Cette methode doit certainement faire perdre des impulsions de temps à autre, en cas de changement de sens non détecté. Mais les tests montrent que ce n'est pas vraiment visible.

Le comptage des impulsions


C'est certainement la partie la plus "sioux" du programme. Et c'est là en tout cas que les contraintes de temps sont les plus fortes.

Il faut déterminer le sens de rotation et sommer les impulsions mesurées par les compteurs.

Pour déterminer le sens, il faut trouver l'état d'une sortie au moment d'un front montant ou descendant d'une autre. Il faut donc d'abord détecter les fronts, puis en déduire le sens.

Voici la partie du code qui se charge de ça :



compteurBoucle = 0;
do {

	DISABLE_INTERRUPTS(GLOBAL);
        signal = PORTA; // Signal 1,1 A4  Signal 1,2 A5 -  Signal 2.1 C0 Signal 2.2 A2
        if (INPUT(PIN_C0)) BIT_SET(signal, 0);
	ENABLE_INTERRUPTS(GLOBAL);

        /* xorSignal : 
                Bit 6 : Sens 1 actuel
                Bit 5 : Difference ancien/nouveau sur le signal 2 du compteur 1
                Bit 4 : Difference ancien/nouveau sur le signal 1 du compteur 1
                Bit 2 : Difference ancien/nouveau sur le signal 2 du compteur 2
                Bit 1 : Sens 2 actuel
                Bit 0 : Difference ancien/nouveau sur le signal 1 du compteur 2
        */

        signal &= 0b00110101;
        old_signal &= 0b00110101;

        if (BIT_TEST(signal ,4))
                BIT_SET(signal ,6); 
        if (BIT_TEST(signal ,5))
                BIT_SET(Old_signal ,6); 

        if (BIT_TEST(signal ,0))
                BIT_SET(signal ,1);
        if (BIT_TEST(signal ,2))
                BIT_SET(Old_signal ,1);

        xorSignal = signal^old_signal;

        if (BIT_TEST(xorSignal  ,6))
                BIT_SET(signal ,7);
        if (sens_1)
                BIT_SET(Old_signal ,7); // On en a besoin car le sens n'est pas forcement mesure a chaque fois

        if (BIT_TEST(xorSignal  ,1))
                BIT_SET(signal ,3);
        if (sens_2)
                BIT_SET(Old_signal ,3); // On en a besoin car le sens n'est pas forcement mesure a chaque fois

        /* xorSignal2 : 
                Bit 7 : Difference ancien/nouveau sur le sens 1
                Bit 3 : Difference ancien/nouveau sur le sens 2
        */

        xorSignal2 = signal^old_signal;

	if (!interruption)  {      
		if (mesure_sens_1) {
	                if ( BIT_TEST(xorSignal ,4) ) {
	                        if (!BIT_TEST(xorSignal,5)) {
	                                sens_1 = BIT_TEST(xorSignal,6); // Xor
	                                if (BIT_TEST(xorSignal2, 7)) {
        	                                changementSens_1 = 1;
                	                        compteurBoucle = 128;
                        	        }                       
	                        }
	                } 
		}
		if (mesure_sens_2) {
	                if (BIT_TEST(xorSignal,0)) {
	                        if (!BIT_TEST(xorSignal,2)) {
	                                sens_2 = BIT_TEST(xorSignal,1); // Xor
	                                if (BIT_TEST(xorSignal2, 3)) {
        	                                changementSens_2 = 1;
                	                        compteurBoucle = 128;
                        	        }                       
	                        }
	                }
		}
	}
	old_old_signal = old_signal;
        old_signal = signal;
        compteurBoucle++;
	interruption = 0;

} while (!BIT_TEST(compteurBoucle,7));


Alors comment tout ceci marche ?

Dans la variable signal, on va stocker les E/S correspondant aux encodeurs (2 signaux par encodeur, soit 4 au total) :

Bit dans Signal Correspond à :
0
Encodeur 2, signal 1
1
0
2
Encodeur 2, signal 2
3
0
4
Encodeur 1, signal 1
5
Encodeur 1, signal 2
6
0
7
0

On recopie ensuite le bit 4 dans le bit 6, et le bit 0 dans le bit 1 :

Bit dans Signal Correspond à :
0
Encodeur 2, signal 1
1
Encodeur 2, signal 1
2
Encodeur 2, signal 2
3
0
4
Encodeur 1, signal 1
5
Encodeur 1, signal 2
6
Encodeur 1, signal 1
7
0

On a ensuite la vaiable Old_signal, qui contient l'état de signal à (t-1), donc à l'itération précédente.

Bit dans Old_signal Correspond à :
0
Encodeur 2, signal 1 à t-1
1
Encodeur 2, signal 1à t-1
2
Encodeur 2, signal 2 à t-1
3
0
4
Encodeur 1, signal 1 à t-1
5
Encodeur 1, signal 2 à t-1
6
Encodeur 1, signal 1 à t-1
7
0

On y recopie dans le BIT 6 l'état actuel du bit 5 de signal. De même, on y recopie dans le BIT 1 l'état actuel du bit 2 de signal.

Bit dans Old_signal Correspond à :
0
Encodeur 2, signal 1à t-1
1
Encodeur 2, signal 2
2
Encodeur 2, signal 2 à t-1
3
0
4
Encodeur 1, signal 1 à t-1
5
Encodeur 1, signal 2 à t-1
6
Encodeur 1, signal 2
7
0

On fait ensuite un XOR (ou exclusif) entre signal et Old_signal. On stocke la nouvelle valeur dans xorSignal.

Le xor entre 2 bits est équivalent à faire le test "différent de" (noté ici "!=")

Donc dans xorSignal il y a :

Bit dans XorSignal (Signal) (Old_signal) Le xor donne :
0
Encodeur 2, signal 1 Encodeur 2, signal 1à t-1 1 si l'Etat de l'Encodeur 2, signal 1 a changé. 0 sinon
1
Encodeur 2, signal 1 Encodeur 2, signal 2 1 si Encodeur 2, signal 2 != Encodeur 2, signal 2. 0 sinon
2
Encodeur 2, signal 2 Encodeur 2, signal 2 à t-1 1 si l'Etat de l'Encodeur 2, signal 2 a changé. 0 sinon
3
0 0 0
4
Encodeur 1, signal 1 Encodeur 1, signal 1 à t-1 1 si l'Etat de l'Encodeur 1, signal 1 a changé. 0 sinon
5
Encodeur 1, signal 2 Encodeur 1, signal 2 à t-1 1 si l'Etat de l'Encodeur 1, signal 2 a changé. 0 sinon
6
Encodeur 1, signal 1 Encodeur 1, signal 2 1 si Encodeur 1, signal 2 != Encodeur 1, signal 1. 0 sinon
7
0 0 0

L'idée ici c'est que en 2 coup de cuillères à pot (et peu d'instructions assembleurs) on a dans XorSignal plusieurs résultats très utiles. On a dans les bits 0, 1, 4 et 6 les changements d'état des sorties des encodeurs. Et qui dit changement d'état dit front montant ou descendant.

De plus les bits 1 et 6 vont donner directement le sens de rotation.

Mais il reste une dernière chose : calculer si le sens de rotation a changé.

Pour cela on met dans le bit 7 de signal le nouveau sens de rotation pour l'encodeur 1 et dans le bit 7 de Old_signal l'ancien sens de rotation. De même avec le bit 3 pour le sens de rotation de l'encodeur 2.

On realise un xor entre signal et Old-signal, et on à le résultat dans xorSignal2. Les bits 3 et 7 de xorSignal2 indiquent donc si le sens de rotation a changé par rapport à t-1.

Ensuite il reste à tester tous les résultats obtenus.

On commence par l'encodeur 1. Y a-t-il eu un changement d'état (front) sur le signal 1 -test du bit 4 de XorSignal-? Si oui, on regarde que le signal 2, lui, n'ait pas changé entre t et t-1 (cela voudrait dire que l'encodeur va trop vite, et que le signal 2 a eu le temps de changer !). Si tout cela est bon, on a le nouveau sens de rotation. On teste même si il a changé (avec le bit 7 de xorSignal2).

Rebelotte ensuite sur l'encodeur 2.

A la fin de la boucle, il n'y a plus qu'à remettre signal dans Old_signal pour la prochaine itération, et ça peut recommencer.

Justement, quand est-ce que l'on s'arrête ?

La boucle de mesure du sens doit s'arrêter de temps à autre. D'abord pour pas ne dépasser la valeur de 256 impulsions comptées sur le timer 8 bits (Cette contraite est vérifiée plus loin ), ensuite pour, à chaque changement de sens, sommer les impulsions dans le bon sens.

La variable compteurBoucle s'incrémente à chaque itération. Au dela de 128, on sort de l'itération. Un changement de sens ou certaines interruptions mettent compteurBoucle directement à 128 (par exemple si on doit calculer le PID, à chaque base de temps).

L'addition des impulsions

Ca peut sembler très simple de sommer ou de retrancher les impulsions à la fin d'une série d'itération suivant le sens de rotation, mais en fait c'est assez compliqué.

Voici le code qui fait ça, juste pour le compteur 1 (même chose pour le compteur 2)

// Compteur 1 
newValueCompteur = GET_RTCC();
if (newValueCompteur>=lastValueCompteur_1) {
	valueCompteur = newValueCompteur - lastValueCompteur_1;
} else {
	valueCompteur = 256 - lastValueCompteur_1 + newValueCompteur;
}

sensTmp = 0;
if (changementSens_1) {
	sensTmp = 1;		
	changementSens_1 = 0;
	if (BIT_TEST(signal ,4)) { /* Car le compteur compte sur un front montant */
		valueCompteur--; //valueCompteur>0 car on vient d'incrementer
		if (valueCompteur == 255) {
			sensTmp = 0;
			valueCompteur = 1;						
		}
	}
}
	
if (sens_1 != sensTmp) 
	compteur_1 += valueCompteur;				
else
	compteur_1 -= valueCompteur;

lastValueCompteur_1 = newValueCompteur;

Au début, on traite d'abord le cas où le compteur a dépassé les 256 et est revenu à 0 (c'est le cas si newValueCompteur<lastValueCompteur_1, et si bien sur le compteur n'a pas augmenté de plus de 256 impulsions).

Ensuite, s'il y a eu changement de sens, on teste l'état actuel du signal 1 de l'encodeur. Si il est à 1, on retranche alors 1 à la valeur du compteur. Pourquoi ? Et bien parce que cette impulsion (qui a été comptée) a été comptée juste au changement de sens, mais dans le mauvais sens. Je sais, c'est pas très clair, et j'ai du faire une tonne de schémas et de tests avant d'en arriver là et que ca marche bien, mais faites-moi confiance, ca marche ! :-P

A noter que compteur_1 est une variable signée sur 16 bits.

Les limites de temps :

Bon alors, considérons un moteur qui va très vite (vitesse max décrite plus haut). Le moteur, bien sûr, ne change pas de sens.

L'itération comprend alors (à peu près) 70 temps de cycle du 16F877 (1 par instruction, 2 pour un saut). Soit une durée de 14 µs.

On est inférieur au 17µs de la 1/2 période à ne pas dépasser, mais pas de beaucoup. En fait en considérant cette limitation-là, on ne peut pas dépasser 35000 impulsion/s sur chacun des 2 encodeurs (soit 1.2 m/s sur Nikoro).

Que se passe-t-il si on dépasse cette vitesse ? Et bien le PIC va voir qu'il y a un front montant sur le signal 1 et les valeurs du signal 2 seront restées identiques ... mais dans le mauvais sens (elles auront en fait eu le temps de changer 2 fois). Du coup le sens sera inversé... ce qui est bien sur plutot facheux.

Nous verons plus loin comment dépasser cette limitation.

Autre limitation, celle qui consiste à compter au maximum 255 impulsions sur le timer 8 bits.

On réalise 128 boucles (dans le pire des cas) de 14µs, soit environ 1,8 ms (en fait l'interruption toutes les 0.5ms se déclanchera bien avant). Dans ces 1.8ms on mesure 255 impulsions au maximum, soit 140 impulsions/ms, soit 140000 impulsions/s. On voit qu'ici on a de la marge !

Le rôle de la variable interruption :

La variable interruption est là pour empêcher le calcul du sens quand il y a une interruption ou un calcul qui ont alongé la durée de la boucle, et qui a pu être à plus de 14µs. Du coup par précaution on fait une boucle "pour rien" où on se contente d'enregistrer l'état des ports pour la boucle suivante.

Il y a une telle interruption toutes les 128 boucles, à chaque interruption de 0.5ms (donc toutes les 35 boucles). Leurs influences sont négligeables, même si l'interruption elle-même peut être relativement longue et faire louper un changement de sens (mais qui sera détecté le coup d'après).

 

Le calcul du PID

Le calcul du PID est quelque chose d'assez bourin. La difficulté majeure est ici de s'accomoder du compilo C qui est assez limité sur les calculs (même si par rapport à l'assembleur c'est le paradis !). En particulier il faut bien faire attention au dépassement de capacités sur les entiers, et aux problèmes de signes.

Les calculs se font ici en 16 bits (avec pour les coefficients une valeur en 8 bits, un octet) avec des résultats intérmédiaires en 32 bits "simulés".

La variable globale "pid" indique si il s'agit du calcul du PID du moteur 1 ou du moteur 2.

signed long sommeLimite32000(signed long &valeur1, signed long &valeur2) {

	signed long temp;

	// Somme Limite a -+ 32000
	temp = valeur1 + valeur2;
	if ((valeur1>0) && (valeur2>0) && (temp<=0))  {
		return 32000;
	} else {
		if ((valeur1<0) && (valeur2<0) && (temp>=0)) 
			return -32000;
		else
			return temp;
	}
}

signed long multiLong(unsigned long &valeur1, short signe_valeur1,  
 unsigned int &valeur2_fort) {

	signed long temp;

	temp = (valeur1>>8) * valeur2_fort;
	if (temp>255) {
		if (signe_valeur1)
			temp = 32760;
		else
			temp = -32760;
	} else {
		temp = (temp<<8);
		temp += ((long)(valeur1&255)) * valeur2_fort ;
		if (!signe_valeur1)
			temp = -temp;
	}
	return temp;
}


// Calcul du PID
#SEPARATE void calculPID() 
{
	signed long e_signe;
	signed long temp;
	signed long temp2;
	signed long PWM_ADD;
	signed long Si;
	signed long max;

	unsigned long  e;
	short signe_e;
	unsigned long  non_signe_Si;
	short signe_Si;

	unsigned int Coef;

	// (PWM) = Kp * e + Te /Ti * (Si += e ) + Td/Te (e(t)-e(t-Te)

	if (pid) {
		if (freinMoteur1) {
			Si1 = 0;
			OUTPUT_HIGH(PIN_B4);
			delay_cycles(1);
			OUTPUT_HIGH(PIN_B3);
			SET_PWM1_DUTY_10b(1000);
			compteur_1_global += compteur_1;
			compteur_1 = 0;
			return;
		}
		if (roueLibreMoteur1) {
			Si1 = 0;
			SET_PWM1_DUTY_10b(0);
			compteur_1_global += compteur_1;
			compteur_1 = 0;
			return;
		}
	}
	else {
		if (freinMoteur2) {
			Si2 = 0;
			OUTPUT_HIGH(PIN_B2);
			delay_cycles(1);
			OUTPUT_HIGH(PIN_B1);
			SET_PWM2_DUTY_10b(1000);
			compteur_2_global += compteur_2;
			compteur_2 = 0;
			return;
		}
		if (roueLibreMoteur2) {
			Si2 = 0;
			SET_PWM2_DUTY_10b(0);
			compteur_2_global += compteur_2;
			compteur_2 = 0;
			return;
		}
	}

	// Calcul de l'erreur
	if (pid) {
		Si = Si1;
		if (sensConsigne1)
			e_signe = compteur_1 - (signed long)consigne1;
		else
			e_signe = compteur_1 + (signed long)consigne1;
		lastError1 = e_signe;
		max = ((signed long)READ_EEPROM(LIMITE_SI_MOTEUR1_fort))<<8;
		max += READ_EEPROM(LIMITE_SI_MOTEUR1_faible);
	}
        else {
		Si = Si2;
		if (sensConsigne2)
			e_signe = compteur_2 - (signed long)consigne2;
		else
			e_signe = compteur_2 + (signed long)consigne2;
		lastError2 = e_signe;
		max = ((signed long)READ_EEPROM(LIMITE_SI_MOTEUR2_fort))<<8;
		max += READ_EEPROM(LIMITE_SI_MOTEUR2_faible);
	}

	// Emission de l'erreur si c'est demande
	if (EnvoisErreur!=0) {
		if (pid == EnvoisErreurPid) {
			bufferEmission2 = e_signe>>8;
			bufferEmission3 = e_signe & 0xFF;
			sizeBuffer = 2;
			EnvoisErreur--;
			if (EnvoisErreur==0) {
				if (EnvoisErreurPid)
					roueLibreMoteur1 = 1;
				else
					roueLibreMoteur2 = 1;			
			}
		}
	}
	

	if (e_signe<0) {
		e = -e_signe;
		signe_e = 0;
	}
	else  {
		e = e_signe;
		signe_e = 1;
	}
	

	// Si : Somme des erreurs. Limite a -+ 32000
	
	Si = sommeLimite32000(Si, e_signe);

	if (Si>max)
		Si = max;
	if (Si<-max)
		Si = -max;

	if (Si<0) {
		non_signe_Si = -Si;
		signe_Si = 0;
	}
	else  {
		non_signe_Si = Si;
		signe_Si = 1;
	}


	// Charge les coef du terme proportionnel depuis l'EEPROM
	if (pid) {
		Coef = READ_EEPROM(EEPROM_KP_MOTEUR1);
	} else {
		Coef = READ_EEPROM(EEPROM_KP_MOTEUR2);
	}

	// Calcul du terme proportionnel
	PWM_ADD = multiLong(e, signe_e, Coef);

	// Charge les coef du terme integral depuis l'EEPROM
	if (pid) {
		Coef = READ_EEPROM(EEPROM_INVKI_MOTEUR1);
	} else {
		Coef = READ_EEPROM(EEPROM_INVKI_MOTEUR2);
	}
	
	// Calcul du terme integral
	temp = multiLong(non_signe_Si, signe_Si, Coef);

	// Somme des termes. Limite a -+ 32000
	PWM_ADD = sommeLimite32000(PWM_ADD, temp);

	if (PWM_ADD>1000)
		PWM_ADD = 1000;
	if (PWM_ADD<-1000)
		PWM_ADD = -1000;

	if (pid) {
		Si1 = Si;
		if (PWM_ADD>=0) {
			OUTPUT_HIGH(PIN_B4);
			delay_cycles(1);
			OUTPUT_LOW(PIN_B3);
			SET_PWM1_DUTY_10b(PWM_ADD);
		}
		else {
			OUTPUT_LOW(PIN_B4);
			delay_cycles(1);
			OUTPUT_HIGH(PIN_B3);
			SET_PWM1_DUTY_10b(-PWM_ADD);
		}	
		compteur_1_global += compteur_1;
		compteur_1 = 0;
		lastPWM1 = PWM_ADD;
	}
	else {
		Si2 = Si;
		if (PWM_ADD>=0) {

			OUTPUT_HIGH(PIN_B2);
			delay_cycles(1);
			OUTPUT_LOW(PIN_B1);
			SET_PWM2_DUTY_10b(PWM_ADD);
			}
		else {
			OUTPUT_LOW(PIN_B2);
			delay_cycles(1);
			OUTPUT_HIGH(PIN_B1);
			SET_PWM2_DUTY_10b(-PWM_ADD);
		}
		compteur_2_global += compteur_2;
		compteur_2 = 0;
		lastPWM2 = PWM_ADD;
	}	

}


/* PWM1 must be between 0 and 1023*/
void SET_PWM1_DUTY_10b (signed long PWM1) {

#BIT CCP1X = 0x17.5
#BIT CCP1Y = 0x17.4
#BYTE	CCPR1L = 0x15	

CCPR1L = (int)(PWM1>>2);
IF (PWM1 & 1)
	CCP1Y = 1;
else
	CCP1Y = 0;

IF (PWM1 & 2)
	CCP1X = 1;
else

	CCP1X = 0;

}

void SET_PWM2_DUTY_10b (signed long PWM2) {

#BIT CCP2X = 0x1D.5
#BIT CCP2Y = 0x1D.4
#BYTE	CCPR2L = 0x1B	

CCPR2L = (int)(PWM2>>2);
IF (PWM2 & 1)
	CCP2Y = 1;
else
	CCP2Y = 0;

IF (PWM2 & 2)
	CCP2X = 1;
else
	CCP2X = 0;
}

 

Rien de bien sorcier donc, même si c'est assez long à mettre au point.

 

Et comment allez plus vite ?

On l'a vu, le comptage des impulsions à proprement parler ne pose pas trop de problème. C'est plutôt la détermination du sens qui est limitative.

Or, en 1ere approximation, la détection du sens est facultative la plupars du temps : en effet, si le robot va vite, il ne va pas repartir dans le sens inverse tout de suite. Il passera forcément par un temps de ralentissement.

D'ou l'idée de ne tout simplement plus mesurer le sens de rotation quand le moteur tourne trop vite.

On va se servir de la base de temps de 0.5ms pour réaliser ce test.

Le programmet met 14µs a peu pres pour déterminer un sens, ce qui correpond donc à la 1/2 période, soit 30 µs pour la période. Sur 0.5ms, soit 500µs, il peut y avoir environ 16 périodes du signal, autant de fronts montants.

Si il y a entre 0 et 16 front montant par 0.5ms le PIC mesure correctement le sens (Entres 16et 32 (approximativement) il ne mesure plus le sens, mais il sait qu'il n'a pas changé).

On ne va pas prendre de risque, et on va une limite de 10 front montant par période de 0.5ms (en fait cette limite est réglabe, cf après).

Quand le PIC va atteindre cette vitesse (sur chaque encodeur indépendamment), il va arréter de déterminer le sens.

Une sécurité comme vérifier que les dernieres valeurs de sens soient les mêmes aurait été mieux, mais je ne l'ai pas fait.

Voir dans le programme complet le code qui gère ca (dans l'interruption), ce n'est rien de très compliqué.

Maintenat que ceci est fait, la limite de vitesse sur la détermination du sens n'est plus.

On a toujours la limite du comptage en 8bits, qui sature a 140000 impulsions/s. Ce qui nous ferait une vitesse théorique de Nikoro à 4.8m/s ... ce qui est largement suffisant. D'autres amélioration sont possible, mais on va s'arréter la pour l'instant. Par contre il se peut que des limites aient été oubliés...

Nota : Des test ont étés réussi avec succes sur des asservissements à 46000 impulsions/s (moteurs maxon professionels). Et il semble que la limite ne soit pas encore atteinte.

La génération des PWM


Ici on utilise le très pratique générateur de PWM hardware du 16F877.

Les 2 PWM utilisent le timer 2 (8 bits)
Le timer 2 est initialisé à 0. Et ils s'incrémente jusqu'à attendre PR2 (valeur 8 bit).
La il repart à 0.
Un prescaler peut être appliqué au timer 2.

La période du PWM est donc égale à : (PR2 + 1) * prescaler * durée d'une instruction (0.2 µs)

Sur la valeur du PWM, le PWM est mis à 1 au début du cycle, jusqu'à ce que le timer 2 atteigne la valeur d'un registre à 10 bits (les 2 bits en plus sont d'une précision supérieur au timer 2 avec ou sans prescaler)

Le temps passé à 1 est donc égal à : (Valeur du registre de 10 bits) * durée d'une instruction/4 (50 ns) * le prescaler du timer 2.

En C on peut utiliser l'exemple suivant :

setup_ccp1(CCP_PWM); // Configure CCP1 as a PWM

// The cycle time will be (1/clock)*4*t2div*(period+1)
// In this program clock=10000000 and period=127 (below)
// For the three possible selections the cycle time is:
// (1/10000000)*4*1*128 = 51.2 us or 19.5 khz
// (1/10000000)*4*4*128 = 204.8 us or 4.9 khz
// (1/10000000)*4*16*128= 819.2 us or 1.2 khz

switch(selection) {
case '1' : setup_timer_2(T2_DIV_BY_1, 127, 1);
break;
case '2' : setup_timer_2(T2_DIV_BY_4, 127, 1);
break;
case '3' : setup_timer_2(T2_DIV_BY_16, 127, 1);
break;
}

set_pwm1_duty(value); // This sets the time the pulse is
// high each cycle. We use the value
// input to make a easy demo.
// the high time will be:
// if value is LONG INT:
// value*(1/clock)*t2div
// if value is INT:
// value*4*(1/clock)*t2div
// for example a value of 30 and t2div
// of 1 the high time is 12us
// WARNING: A value to high or low will
// prevent the output from
// changing.


Ici clock est la fréquence du quartz, donc 4 fois la fréquence d'une intructions.

Exemple de PWM
On va prendre PR2 = 255.
Le timer 2 sans prescaler.
La période du PWM est donc 256 * 0.2 µs = 51.2 µs, soit 19,531 Khz
(Valeur convenable pour un PWM...)

Le PWM est ensuite fixé. C'est une valeur entre 0 (pas de signal) et 1024 (signal toujours à 1).
La précision du PWM lui-même semble bien suffisante.

En C :
setup_ccp1(CCP_PWM);
setup_timer_2(T2_DIV_BY_1, 255, 1);

puis :
set_pwm1_duty(PWM1);


Equivalent en assembleur :
On a les registres :
PR2
CCPR1L ( CCPR2L)
CCP1CON

A l'init
Si on veut une précision sur 8 bits PR2=3F
Si on veut une précision sur 10 bits PR2=FF

Le réglage du PWM :

Sur 8 bits c'est simple
le PWM directement dans CCPRxL ( 0 pour 0%, 255 pour 100%)

Sur 10 bits même choses avec 2 bits de plus. dans CCP1CON

On peut aussi régler PR2 pour commander directement le PWM en %.

Utilisation du timer2 pour la base de temps :
Il est utile d'utiliser le timer 2 (les autre sont en compteur) pour avoir une base de temps précise.
Le timer 2 à un postcaler qui permet de faire des interruption tous les X remises à 0. Valeurs possibles : de 1:1 à 1:16 par pas de 1.
On à vu que les base de temps sont des multiples de 1ms.
En choisissant le PR2 à 249, on obtient une période du PWM de 50 µs (20 Khz) de période.
Avec un postcaler à fixé à 10, le timer2 va générer une interruption toutes les 500 µs, soit toutes les 0.5 ms, assurant ainsi une grande précision pour la base de temps.
Cette solution est très efficace. Elle permet de plus de prendre en compte les traitements comme le calcul du PID dans le temps de la base de temps.

En C :
setup_ccp1(CCP_PWM);
setup_timer_2(T2_DIV_BY_1, 249, 10);

puis :
set_pwm1_duty(PWM1);

Et la gestion de l'interruption.

Cela donne une valeur de PWM entre 0 (pas de signal) et 249 *4 = 996 (signal toujours à 1).
En gros entre 1 et 1000, donc en 10eme de %.

Le stoquage des constantes du PID en EPROM

Les constantes du PID (Kp et Ki) ansi que d'autres paramètres sont copiées dans l'eeprom. Le pic y a un accès direct et rapide.
Tout changement est pris immédiatement en compte dans les calculs suivant.
Voici le détail de leur emplacement :

Position dans l'EEPROM Signification
Octet 0 Kp moteur 1
Octet 1 Kp moteur 2
Octet 2 (1/Ki) moteur 1
Octet 3 (1/Ki) moteur 2(partie entière)
Octet 4 Limite de SI moteur 1 (poid fort)
Octet 5 Limite de SI moteur 1 (poid faible)
Octet 6 Limite de SI moteur 2 (poid fort)
Octet 7 Limite de SI moteur 2(poid faible)
Octet 8 Limite haute de la mesure moteur 1
Octet 9 Limite basse de la mesure moteur 1
Octet 10 Limite haute de la mesure moteur 2
Octet 11 Limite basse de la mesure moteur 2


Donc Kp moteur 1 = octet0
Donc Kp moteur 2= octet1
Donc (1/Ki) moteur 1 = octet2
Donc (1/Ki) moteur 2= octet3

La limite de SI pour le moteur 1 est 256*octet4+ octet5
La limite de SI pour le moteur 2 est 256*octet6+ octet7

Ces limites de SI correspondent a la somme des erreus maximum (terme intégral dans le PID). Globalement, la limite doit etre suffisante pour que meme dans le cas d'une commande minimale le moteur puisse aller a fond si il est bloque (ce qui correpond ici a la formule 1000 (PWM max) = Si*(1/Ki) + 1*Kp)), et elle doit etre majore pour ne pas depasser inutilement la limite max du PWM.
Une formule simple que je vous propose pour la calculer : Limite de Si = (1000-Kp)/(1/Ki)
Attention, si ces limites sont laissés à 0, Si sera toujours nul, donc il n'y aura plus de terme proportionnel !

Concrernant la limite des mesures (octets 8 à11), il s'agit de la limite en nombre de pas par 0.5/ms a partir de laquelle la mesure de sens sur le moteur ne se fait plus. il s'agit d'un hystérésis, donc la limite "haute" est celle a laquelle la non-mesure du sens de rotation s'enclanche. La limite basse est celle ou la mesure reprends.
Les valeurs conseillés sont eviron 10 pour la mesure haute, et 8 pour la basse.
Si vous metez 0 sur la mesure haute (de l'un ou l'autre moteur) le test ne se fait plus, et la mesure se fera tout le temps. Vous pouvez le faire sans problème si vous etes sur que les impulsions des encodeurs reçues par le PID iront moins vite que 10 pas par 0.5 ms.

 

Ce qu'il reste à faire ...

Le PID semble fonctionne très bien, aussi bien dans les vitesses rapides que lentes.

Le test sur Nikoro est concluent.

Le programme est maintenant capable en plus de recevoir les ordres par liaison série. On peut régler ainsi la vitesses des deux moteurs. On peut aussi modifier certains paramêtres comme les coef du PID, et avoir des retours d'information.

Il reste à faire une version I2C et ISA.

 

 

La communication avec le donneur d'ordre

Le PIC 16f877 prends ses ordre par liaison serie (plus tard I2C ou ISA).

Les ordres prennent la forme de mots de 4 octects

  • Le 1er octet est toujours 0 (c'est pour être sur de ne pas se décaler dans la lecture des octets, surtout en série ou I2C). Aucun autre octet ne dois avoir la valeur 0 (il y a des exeption).
  • Le 2ème octet est la commande
  • Les 3ème et 4ème octets sont les données.

Les commandes possibles sont :

(nota : la valeur "XXX" signifie tout sauf 0)

1er octet 2ème octet 3ème octet 4ème octet Description
0 255 XXX XXX Frein d'urgence (des que les 2 premiers octets sont lu, le PIC met les deux moteurs en court-circtuit).
0 254 XXX XXX Roue libre. Aucune puissance n'est appliqué aux deux moteurs.
0 1

base de temps en ms *2 (0 interdit).Ajouter +1 (nombre impair) pour le sens de rotation direct. Laisser pair pour le sens de rotation inverse.

nombre de pas pendant la base de temps (0 interdit)

vitesse moteur 1.

Indique au PIC d'asservir le moteur 1 à "nombre de pas" pendant la "base de temps".

Par exemple 21 et 50 sur les 3ème et 4ème octets indiquent au PIC d'asservir la vitesse du moteur à 50 pas par tranche de 10ms, sens direct.

0 1 255 XXX moteur 1 en court circuit (freinage)
0 1 254 XXX moteur 1 en roue libre.
0 2 base de temps en ms*2 (0 interdit) +1 si sens de rotation direct. nombre de pas pendant la base de temps (0 interdit) idem moteur numéro 2
0 2 255 XXX moteur 2 en court circuit (freinage)
0 2 254 XXX moteur 2en roue libre.
0 3 base de temps en ms*2 (0 interdit) +1 si sens de rotation direct. nombre de pas pendant la base de temps (0 interdit) idem mais envois la commande sur les deux moteurs en même temps
0 4 base de temps en ms*2 (0 interdit) +1 si sens de rotation direct. nombre de pas pendant la base de temps (0 interdit)

idem mais envois la commande sur les deux moteurs en même temps.

Contrairement a "3", le moteur 1 trourne bien dans le sens indiqué, mais le moteur 2 tourne dans le sens inverse.

0 10 Adresse EEPROM (0 à 255) Valeur EEPROM (0 à 255)

Permet de modifier une valeur de l'EEPROM du PIC. Sert en particulier à changer les coeficiants du PID du PIC (voir ci-dessus pour leur emplacement dans l'EEPROM).

Les valeurs 0 sont autorisés ici.

0 11 Adresse EEPROM (0 à 255) XXX

Demande au PIC de retourner la valeur de l'octet contenu dans l'adresse de l'EEPROM

La valeurs 0 pour l'adresse est autorisée ici.

0 20 255 XXX Demande de retour des valeurs des compteurs
0 30 255 XXX Demande de retour des PWM sur les moteur.
0 31 255 XXX Demande de retour des erreurs sur les moteur.
0 51 base de temps en ms*2 (0 interdit) +1 si sens de rotation direct. nombre de pas pendant la base de temps (0 interdit) Fait la même chose que le 0,1,xxx,xxx, mais y ajoute un renvoit d'information utile pour le reglage des coeficients (voir ci-dessous)
0 52 base de temps en ms*2 (0 interdit) +1 si sens de rotation direct. nombre de pas pendant la base de temps (0 interdit) Fait la même chose que le 0,2,xxx,xxx, mais y ajoute un renvoit d'information utile pour le reglage des coeficients (voir ci-dessous)
0 60

Si 1 -> PORT A
Si 2 -> PORT B
Si 3 -> PORT C
Si 4 -> PORT D
Si 5 -> PORT E

XXX Demande le retour de l'information sur la valeur du port (l'état des bits d'E/S)

Dans tous les cas le PIC renvois une valeur "0" apres les 4 octets pour confirmer qu'il a bien recu la commande (meme si il ne l'a pas forcement comprise)

Il renvoit à l'inverse "255" si il y a un problème (overflow du buffer...)

Pour les commandes la base de temps ne doit pas être inférieur à 4ms (soit une valeur de 8 ou 9 suivant le sens).

Explication sur le code 0,20,x,x :

Il demande au PIC un retour d'information sur la valeur des compteurs. Le pic retourne ces valeurs sous la forme de 4 octets qui se suivent (apres l'octet 0 pour dire qu'il a bien recu la commande soit 5 octect de retour en tout). Les 2 premier sont le compteur du moteur 1. Les 2 autres ceux du moteur 2. Ces valeurs sont signés. Ces valeurs sont aussi incrémentés toutes les bases de temps, et ne sont jamais remises à 0. Au programme de demander les information suffisemment souvent pour ne pas dépasser 32768 pas en plus on en moins. Le PIC ne recevra aucune commande avant l'emission complete des 4 octets de reponses, qui ont lieu toutes les ms à peu près chacun (Environ 5ms pour l'émission complète, code 0 compris).

Dans le cas du code 0,10,xxx,xxx (écriture dans l'EEPROM) il faut bien savoir que le PIC ne peut écrire très rapidement dans l'EEPROM. Il a besoin de plusieurs ms pour le faire. Donc si on envois plusieurs commandes pour modifier l'eeprom a la suite, si la 1ère n'est pas fini alors que la 2ème est recu, le PIC va retourer le code d'erreur 255 (au lieu de 0). Il faut donc renvoyer la commande.

Pour le code 0,11,adresse,xxx, le PIC va d'abord retourner un "0" comme accusé de réception, puis il va envoyer l'octet correspodant au contenu de l'adresse de l'EEPROM.

Pour les codes 0,51,xxx,xxx et 0,52,xxx,xxx, la commande envoye pour les moteurs 1 et 2 est la même que celle des codes 0,1,xxx,xxx ou 0,2,xxx,xxx. La différence est que le PIC ne va l'appliquer que pendant 256*la base de temps, et qu'il va dans ce laps de temps renvoyer tous les (base de temps) ms 2 octets, représentant l'erreur qu'il a mesuré sur le moteur par rapport a la consigne (le "e" de la formule du PID). Ce retour est très pratique pour régler efficacement les coeficients du PID, en suivant leur évolution.

Voici ici 2 exemples de courbes générés avec ce retour. Il s'agit d'une commande de vitesse (l'un a vide, l'autre en change) à une vitesse de 100 pas par 4ms avec des coeficients différents.

Exemple1 exemple2

Chaque trait sur l'abscisse correspond a 4ms. Chaque trait sur l'ordonnée correspond à une erreur de 5 pas. Comme vous pouvez voir, bien régler ses coéficients permet d'asservir mieux et plus vite !

Note : Il faut évidemment que les codes ne soit pas 0,51,254,xxx ou 0,51,254,xxx, sinon il n'y a pas d'ereur à renvoyer.

Pour le code 0,60,port,xxx, le PIC va d'abord retourner un "0" comme accusé de réception, puis il va envoyer l'octet correspodant à la valeur du port d'E/S demandé.

 

Les méthodes de calcul de coéficients du PID

Le programme étant réalisé, il y a une état crucialle à faire : régler les coéficiens du PID, Kp et Ki (ici 1/Ki pour simplifier).

Il y a beaucoup de méthode pour cela, mais n'étant pas un spécialiste de la question je m'abstiendrais dans un premier temps de donner des conseils.

Sur Nikoro j'ai réglé "à la louche" Kp à 10 et 1/Ki à 5.

Le retour de l'évolution de l'erreur (voir ci-dessus) permet de régler ces coéficiens par tatonement, juste en essayant d'obtenir la meilleure courbe.

Sur des moteurs maxon de Haku j'ai un Kp de 25 et 1/Ki a 5.

 

Le programme

Voici le programme.

Il est directement utilisable en liaison série. La liaison I2C et ISA étant encore à réaliser.

Ce n'est certainement pas une version définitive, mais c'est déjà très complêt.

Les tests montre que le PID marche nickel sur des petites et grandes vitesses (45000 impulsion/s, asservissement à 200 impulsion toutes les 4ms à peu près.

Regardez sur la page de Haku une video (4.5Mo, au format MOV) qui montre le PID sur PIC au travail !



 



Complétez cette page, posez vos questions et remarques ici : WiKiFri

Page http://fribotte.free.fr/bdtech/PidSurPic/PidSurPic2.html modifiée le 13/09/2004.
Copyright fribotte@free.fr, libre de droit pour toute utilisation non commerciale.
Reproduction autorisée par simple mail