Le code? quel code?
La carte a été développée pour faire des essais avec plusieurs capteurs : un gyroscope, un accéléromètre et un système à télémètre infra rouge. Le code a donc été développé pour pouvoir faire diverses combinaisons. On peut même en changer à l'aide de la télécommande sans arrêter le robot.
Bien que le fonctionnement basé uniquement sur le gyroscope soit satisfaisant, on décrit içi le code complet.
La carte utilise un 18F4550 cadencé à 40 MHz
Gyroscope
Pour traiter le gyroscope, il faut lui envoyer des signaux calibrés et les analyser en sortie. Le signal émis est une impulsion de 1msec toutes les 10msec.
On utilise le CCP2 en mode compare pour envoyer le signal. L'interruption est très simple :
/*******************************
/ Interrupt Compare
/******************************/
#INT_CCP2
void ccp2_isr (void) {
if (input(GYRO_PWM)){ // si on envoie un 1
CCP_2 += SEND_ON; // durée d'envoi d'un 1
} else { // si on envoie un zéro
CCP_2 += SEND_OFF; // durée d'envoi d'un zéro
}
}
Noter qu'on ne réinitialise pas le compteur mais on se contente d'ajouter le temps à compter.
Simple... Il faut maintenant analyser le signal en retour du gyroscope... Pour ce faire, on va utiliser CCP1 en mode capture pour avoir les temps au front montant puis descendant.. Le code en interrupt est le suivant (les explications suivent) :
/********************************
/ Interrupt capture
/*******************************/
#INT_CCP1
void ccp1_isr (void){
Mauvais = true; // invalider le calcul de l'accéléromètre
if (Wait_On){ // réception d'un début d'impulsion
GyroKo=false;
Wait_On=false;
Gyro_Debut= CCP_1;
setup_ccp1(CCP_CAPTURE_FE);
}else{ // réception d'une fin d'impulsion
Wait_On=true;
Duree_A = CCP_1 - Gyro_Debut;
if (Duree_A <0) Duree_A = -Duree_A;
setup_ccp1(CCP_CAPTURE_RE);
//
if(Gyro_Toggle){ // premier passage
Gyro_Toggle=false;
if(!GyroKo)Duree_B= Duree_A;
else Duree_B=Duree_Gyro/2;
Accel_Flag=true; // lancer l'accéléromètre 20msec
}else{ // second passage
Gyro_Toggle = true;
if(!GyroKo)Duree_Gyro = Duree_B+Duree_A;
else Duree_Gyro = Duree_B +(Duree_Gyro/2);
Flag_Traitement = true; // lancer le traitement 20msec
}
}
}
Le booléen Mauvais sert à invalider le calcul de l'accéléromètre (qui est perturbé par cette interruption).
Gyro_début mémorise la valeur du compteur de temps au front montant. Duree_A contient donc la durée de l'impulsion.
On va faire 2 mesures et donner une réponse toutes les 20msec. Noter que GyroKo est levé quand une interruption I2C a eu lieu et donc on invalide la mesure en cours.
Enfin, pour maitriser le déroulement des taches, on lance (en base) le calcul de l'accéléromètre et le traitement de l'attitude toutes les 20msec mais chacun étant décalé de 10 msec par rapport à l'autre.
Accéléromètre
L'accéléromètre émet vers le micro deux (axes X et Y) trains de signaux dont le rapport cyclique permet de déduire l'accélération sur chaque axe.
La mesure est lancée en niveau de base toutes les 20msec (lors de la fin de la première mesure du gyro -voir ci dessus-).
On ne va pas faire compliqué et simplement boucler pour calculer les durées respectives (par axe de mesure) du 1 et du 0 donc le rapport cyclique R . On calculera ensuite la valeur de l'accélération par une équation linéaire de type AR+B. Les valeurs A et B sont calculées lors de la mise au point et vont correspondre à unevaleur de 1G (X tourné vers le sol) et 0G (X horizontal).
Pour avoir une mesure fiable, on annule toutes celles qui auraient été perturbées par des interruptions : on alloue donc au total 6 tentatives de mesures.
Voici le code de la fonction de lecture de l'accéléromètre
void Lit_Accel() {
int i;
float G_Temp;
Accel_X_OK=false;
Accel_Z_OK=false;
Accel_Flag= false; // acquittement.
for (i=0;i<5;i++){ // 5 essais de réception X max (on attend entre 1 et1.5msec par essai)
Mauvais = FALSE;
if (input(ACCEL_X)){ // si entrée = 1, on va mesurer OFF puis ON
while(input(ACCEL_X)); // attente passage à 0
Accel_1= get_timer3(); // date passage à 0
while(!input(ACCEL_X)); // attente passage à 1
Accel_2= get_timer3(); // date passage à 1
Accel_X_OFF = Accel_2-Accel_1;
while(input(ACCEL_X)); // attente passage à 0
Accel_3= get_timer3(); // date passage à 0
Accel_X_ON = Accel_3-Accel_2;
}else{ // si entrée=0, on va mesurer ON puis OFF
while(!input(ACCEL_X)); // attente passage à 1
Accel_1= get_timer3(); // date passage à 1
while(input(ACCEL_X)); // attente passage à 0
Accel_2= get_timer3(); // date passage à 0
Accel_X_ON = Accel_2-Accel_1;
while(!input(ACCEL_X)); // attente passage à 1
Accel_3= get_timer3(); // date passage à 1
Accel_X_OFF = Accel_3-Accel_2;
}
if (Mauvais==false) { // pas d'interruption : mesure bonne
Accel_X_OK = true;
break; // passage à Z
}
}
for (i=i;i<6;i++){ //pas plus de 6 essais au total **********************
Mauvais = FALSE;
if (input(ACCEL_Z)){ // si entrée = 1, on va mesurer OFF puis ON
while(input(ACCEL_Z)); // attente passage à 0
Accel_1= get_timer3(); // date passage à 0
while(!input(ACCEL_Z)); // attente passage à 1
Accel_2= get_timer3(); // date passage à 1
Accel_Z_OFF = Accel_2-Accel_1;
while(input(ACCEL_Z)); // attente passage à 0
Accel_3= get_timer3(); // date passage à 0
Accel_Z_ON = Accel_3-Accel_2;
}else{ // si entrée=0, on va mesurer ON puis OFF
while(!input(ACCEL_Z)); // attente passage à 1
Accel_1= get_timer3(); // date passage à 1
while(input(ACCEL_Z)); // attente passage à 0
Accel_2= get_timer3(); // date passage à 0
Accel_Z_ON = Accel_2-Accel_1;
while(!input(ACCEL_Z)); // attente passage à 1
Accel_3= get_timer3(); // date passage à 1
Accel_Z_OFF = Accel_3-Accel_2;
}
if (Mauvais==false) { // pas d'interruption : mesure bonne
Accel_Z_OK = true;
break; // passage à Z
}
}
if(Accel_X_OK && Accel_Z_OK){ // seulement en cas de succès!
G_Temp= Ax*( ((float)(Accel_X_ON)/(float)(Accel_X_ON +Accel_X_OFF)) - Gx0);
if (G_Temp>1) G_Temp=1;
if (G_Temp<-1) G_Temp=-1;
G_X=(G_X_Old3+G_X_Old2+G_X_Old1 + G_Temp)/4.0;
G_X_Old3=G_X_Old2;
G_X_Old2=G_X_Old1;
G_X_Old1=G_Temp;
G_Temp= Az*( ((float)(Accel_Z_ON)/(float)(Accel_Z_ON +Accel_Z_OFF)) - Gz0);
if (G_Temp>1) G_Temp=1;
if (G_Temp<-1) G_Temp=-1;
G_Z=(G_Z_Old3+G_Z_Old2+ G_Z_Old1+ G_Temp)/4.0;
G_Z_Old3=G_Z_Old2;
G_Z_Old2=G_Z_Old1;
G_Z_Old1= G_Temp;
}
/* On calcule l'angle en radians
* angle = positif si penché en avant, négatif si penché en arrière
* -G_X,+G_Z si le CI est horizontal
* +G_Z,+G_X si le CI est vertical
* Angle_Accel = atan2( -G_X, +G_Z );
*/
Angle_Accel = atan2(+G_Z,+G_X);
// et enfin, on va positionner l'alarme en cas d'angle hors limite
if (Angle_Accel>Hors_Limite ||Angle_Accel<-Hors_Limite ){
Accel_Alarme=true;
}else if (Accel_Alarme){ // on ne sort que si l'angle est faible
if (Angle_Accel<Restart && Angle_Accel>-Restart)
Accel_Alarme=false;
}
Vous noterez qu'on intègre les lectures (G_X_Old et G_Z_Old) : l'accéléromètre est un composant très bruyant et sensible à la moindre vibration !
Vous noterez aussi le déclenchement d'une alarme si l'angle devient trop important et également le retour en fin d'alarme si l'angle devient très faible -Hors_Limite est fixé à 0.5 degrés-.
Détecteur de distance
Pffff le code est trivial puisque les détecteurs donnent une tension qui varie en fonction de la distance. Et comme les détecteurs sont symétriques et "regardent" à 45 degrés, on va dire grossièrement que l'angle varie en fonction de la différence de distance détectée par chaque capteur :
void Lit_Distance() {
set_adc_channel(D_AVANT);
delay_us(30);
Dist_Av = read_adc();
set_adc_channel(D_ARRIERE);
delay_us(30);
Dist_Ar = read_adc();
Angle_Opto= ((float)(Dist_Ar-Dist_Av-trimSharp))*0.00275; //résultat en radians
}
Voilà .. on a maintenant les résultats des capteurs gyroscope, accéléromètre et capteurs optique : Tout reste encore à faire !!!
Les trims
Une chose est d'acquérir les infos des capteurs, une autre est de les calibrer ! Mais c'est possible enr éfléchissant un peu :
Tout d'abord, il est "évident" que la vitesse moyenne de rotation détectée par le gyroscope est nulle
Ensuite, on va passer quelques secondes (16 pour être précis) après l'allumage du robot en le tenant bien vertical, ce qui va permettre de calibrer l'accéléromètre et les détecteurs optiques.
La différence entre les 2 points précédents est que la vitesse moyenne du gyro est calculée tout le temps (intégration glissante) alors que les calibrage accéléromètre et opto ne sont faits qu'à l'allumage.
Point besoin d'être parfaitement vertical : on a vu dans le logiciel d'asservissement qu'il existe une formule qui tient compte de la pente éventuelle pour tenir quand même debout : elle corrige également les petits écrts de calibrage.
Le code des trims est identique, aussi je n'en cite qu'un seul : on cumule les valeurs lues dans une table de float. Chaque float cumule une seconde de mesure. Puis, à chaque seconde, on fait le total des N dernières secondes pour en extraire la valeur moyenne glissante qui servira de trim.
Voici le trim de la vitesse du gyro (toujours actif)
/**
* Calculer le trim de la vitesse du gyro
*
**/
if (Gyro_comptA==0) Gyro_vit[Gyro_comptB]=0; // effacer avant première intégration
Gyro_vit[Gyro_comptB] += Duree_Gyro; // on intègre
Gyro_comptA ++; // compteur 1 sec
if (Gyro_comptA>49){ // on a intégré 50 fois 20msec=1sec?
Gyro_comptA=0; // redémarrer le compteur 1sec
Gyro_comptB ++; // incrémenter le compteur d'entrée
if (Gyro_comptB>15) Gyro_comptB=0; // modulo 15
total=0; // calcul de la vitesse ces 15 dernières secondes
for (i=0;i<16;i++)total +=Gyro_vit[i]; // calcul de la vitesse moyenne
total = (total>>4); // vitesse sur 1 seconde
/*
* si on est en phase de calibrage et que le filtre est stable,
* alors, on va terminer la phase et initialiser l'angle et le bias du gyro
*/
if(Calibrage){
Delta_Trim = (float)(RECEIVE) -((float)(total)/50.0) -Gyro_Trim;
if((Delta_Trim>-1) && (Delta_Trim<1)) Calibrage=false;
}
Gyro_Trim= (float)(RECEIVE) -((float)(total)/50.0); // maj trim de vitesse tout le temps
}
/**
* En déduire la vitesse angulaire du gyroscope
*
*la variable Duree_Gyro contient la somme des 2 dernières mesures
**/
Vitesse_Gyro = ((float)(Duree_Gyro - RECEIVE+ Gyro_Trim))*SENSIBILITE; // en radians par seconde
Vitesse_Gyro contient donc la vitesse débiaisée... même si le gyroscope dérive, cette vitesse restera vraie.
Les trims de l'accéléromètre et de l'opto sont identiques mais ne sont calculés que tant que Calibrage est vrai... pendant que l'on tient sagement le robot à la verticale (noter que dans ces derniers cas, l'intégration se fait sur 8 secondes et non 16 donc elle se termine toujours avant que calibrage devienne faux.
Et maintenant ? : Nous avons la vitesse angulaire, l'angle de l'accéléromètre, celui calculé à partir des sharps...Pour être complet et s'amuser sur les combinaisons, il nous manque l'angle calculé à partir de l'intégration de la vitesse du gyro...
L'angle à partir du gyro
Il s'agit d'intégrer la vitesse angulaire... tout en sachant que le gyro est très bas de gamme et va donc dériver terriblement !
On va donc calculer la dérive de cet angle en prenant comme principe que, lorsque le robot est debout, il est en moyenne vertical.....
/***************************************
* Angle GYRO
****************************************/
Angle_Gyro += Vitesse_Gyro*Filtrage_Period; //intégration pour calculer l'angle si on est pas en alarme
/**
* calculer le trim de l'angle du gyro (on le fait sur 8 secondes)
*
**/
if (Calibrage) Angle_Gyro=0; // maintenir angle nul durant le calibrage
if (!Accel_Alarme){ // s'il n'y a pas d'angle excessif, calculer
if (Ang_gyrA==0) Ang_zero_gyr[Ang_gyrB]=0; // effacer avant première intégration
Ang_zero_gyr[Ang_gyrB] += Angle_Gyro; // on intègre
Ang_gyrA ++; // compteur 1 sec
if (Ang_gyrA>24){ // on a intégré 25 fois 20msec=1sec?
Ang_gyrA=0; // redémarrer le compteur 1sec
Ang_gyrB ++; // incrémenter le compteur d'entrée
if (Ang_gyrB>15) Ang_gyrB=0; // modulo 15
somme=0; // calcul de l'angle moyen 15 dernières secondes
for (i=0;i<16;i++)somme +=Ang_zero_gyr[i]; // calcul de l'angle moyen
Trim_Angle_Gyro= -somme/400.0; // maj trim gyro(on divise par 25 fois 16)
}
}else{ // s'il y a angle excessif, reset du trim et du calcul
for(i=0;i<16;i++) Ang_zero_gyr[i]= 0 ;
Trim_Angle_Gyro=0;
Angle_Gyro=Angle_Accel;
}
Noter que si le robot est tombé (l'accéléromètre montre un angle excessif, on s'empresse de remettre à zéro les données de calcul.. tout sera redémarré quand l'accéléromètre détectera à nouveau un passage à la verticale
La suite
Voilà pour aujourd'hui...
Nous possédons toutes les données traitées :
- Vitesse angulaire
- Angle donné par l'accéléromètre
- Angle donné par les capteurs optos
- Angle calculé à partir de l'intégration de la vitesse angulaire
Au prochain numéro, la fin qui comprend le filtrage Kalmal et le choix des données à transmettre à l'asservissement.....
