Latest revision as of 10:33, 9 April 2014

Localisation par balisage infrarouge

Retour à User:Mael
Cette page présente mes travaux actuels sur la fabrication d'un système de balisage infrarouge pour mobile.
Le système contient les fonctions suivantes :

• Emission infrarouge de 4 balises autonomes,
  
• Réception infrarouge sur une tourelle,
  
• Contrôle d'un moteur pas à pas d'entrainement de la tourelle.
  

En plus de la fonction de triangulation associée au balisage, le système intègre en permanence deux entrées codeuses, assurant ainsi l'odométrie du mobile.
Les données d'odométrie sont ensuite intégrées aux données de balisage en permanence par l'intermédiaire d'un filtre de Kalman.

Un troisième volet de ce projet est le calcul en permanence d'une "void area", basée sur un réseau de capteurs ultrason déployés sur le mobile.

Dans un second temps, un autre projet viendra compléter celui-ci avec la réalisation d'un système de contrôle commande des moteurs (en DC, stepper DC puis en BLDC) avec un calcul de trajectoire basé sur les splines de Catmull-Rom.

Système de balisage infrarouge

Le principe de balisage infrarouge consiste à utiliser des balises émettant un signal infrarouge que va détecter un plot central, déployé sur une structure mobile roulante par exemple.
Le concept est développé dans le cadre de la coupe de France de robotique, et permet de disposer facilement d’une information de localisation sur la table de 3x2m.

Le but est d’obtenir la meilleur estimation de x et y afin d’avoir la position du robot sur la table, ainsi que son orientation.
Le principe appliqué est celui de la triangulation, basée sur une mesure d’angle (il existe le même principe avec les distances, tout n’étant au final qu’une application des équations de trigonométrie).
Le choix du balisage infrarouge est lié au faible coût de la mise en œuvre (une centaine d’euros tout compris) et à la relativement grande précision du système.
Il sera couplé, dans la suite de ce document, à un dispositif d’odométrie et un dispositif de sonars permettant d’affiner la position courante du robot et de créer une « void area » autour du robot indiquant les zones accessibles ou non au robot.

Les concepts définis par la suite s'appuient principalement sur deux éléments :

• Microchip DsPic (33FJ128MC802)
• SSV DNP5370 (Blackfin)

Le détail de l'utilisation de ces composants est donné dans les pages associées.

Triangulation

Le principe de la triangulation est de mesurer trois angles entre des points fixes et un point mobile, et de retrouver, connaissant la position des points fixes, la position du point mobile. La manœuvre est limité à la détection d’objets à l’intérieur de la zone formée par les points fixes (c’est peut-être possible en dehors, mais je n’ai pas fait l’analyse, pas besoin).
Le nombre limite de points fixes est au minimum de 3, dans le cadre de l’étude 4 seront utilisés (par extension de la version à 3 points). Plus il y a de points fixes, meilleure est la précision, mais plus long seront les traitements.
Tout d’abord, définissons quelques variables. Le schéma ci-dessous représente la table sur laquelle est porté un repère orthonormé qui va servir de base aux calculs. Deux angles sont définis :

• θAB,
• θAC.

Le calcul de ces deux angles sera précisé plus bas. Pour l’instant, partons du principe qu’ils sont connus. Par triangulation, nous allons calculer xM et yM en calculant un certain nombre de variables de cette figure qui nous donnerons, in fine, les coordonnées du point M.
Par analyse du schéma, on pose :

xM = AM x Cos[a]

yM = AM x Sin[a]

Nota : pour ceux qui préfèrent travailler en polaire, il est possible d’utiliser AM (ie r) et a (ie θ) directement, par exemple pour trianguler la position d’un outil sur un bras SCARA.
Le calcul de la variable AM se base sur les trois formules suivantes :

• Loi des sinus : MB /Sin[a] = MA/Sin[Pi - θAB - a] = AB/Sin[θAB]
• Sin[Pi - x] = Sin[x]
• Sin[a+b] = Sin[a] x Cos[b] + Sin[b] x Cos[a]

On définit ainsi :

MB = (AB x Sin[a]) / Sin[θAB] = (AM x Sin[a]) / Sin[Pi - θAB - a]

Avec : Sin[Pi - θAB - a] = Sin[θAB] x Cos[a] + Sin[a] x Cos[θAB]

D’où, tous calculs fait :

Le calcul de la variable [a] se base sur les formules suivantes :

• Loi des sinus : MB /Sin[a] = MA/Sin[Pi - θAB - a] = AB/Sin[θAB]
• Sin[Pi - x] = Sin[x]
• Sin[a+b] = Sin[a] x Cos[b] + Sin[b] x Cos[a]
• Sin[PI/2 + x] = Cos[x]
• Cos[a-b] = Cos[a] x Cos[b] + Sin[a] x Sin[b]

Dans le schéma ci-dessus, on applique la formule des sinus :

AC / Sin[θAC] = AM / Sin[{ACM}]

AB / Sin[θAB] = AM / Sin[{MBA}]

Sachant que les angles :

{ACM} = PI/2 – a - θAC

{MBA} = PI – a - θAB

AC / Sin[θAC] = AM / Cos[a - θAC]

AB / Sin[θAB] = AM / Sin[a + θAB]

Il ne reste plus qu’à identifier [a] en joignant les équations en AM.
  Tous calculs faits :

(AC x Tan[θAC]) x Cos[a] + AC x Sin[a] = (AB x Tan[θAB]) x Sin[a] + AB x Cos[a]

Le calcul de [xM, yM] est réalisé en permanence. La précision de la position dépendra en grande partie de la précision de la mesure des angles, qui est liée à la technologie de mesure.
Sur la base de ce qui précède, et sans tenir compte pour l’instant des erreurs de mesure, la triangulation est possible en mesurant au moins deux des quatre angles formées par les 4 balises aux 4 coins de la table et la tourelle de réception.

Détail des applications

Le chapitre suivant va décrire plusieurs applications complètes et unitaires à base de microcontrôleur DsPIC33F, qui reprennent toutes les fonctions vues ci-dessus. Chacune de ces applications peut être reprise dans un projet quelconque sans adaptation particulière, en tenant compte toutefois de l’ICD associée à chaque carte.
Les applications suivantes sont présentées :

• Emetteur infrarouge,
• Récepteur infrarouge,
• Lecture sonars,
• Driver moteur pas à pas,
• Driver servomoteur.

Chaque application va utiliser le même type de µC, des DsPic33FJ64MC802, et la même communication en ECAN pour transmettre ou recevoir des données du Blackfin ou de tout autre composant disposant de module CAN.
Les dimensions proposées sont également standardisées, pour permettre l’intégration dans un fond de panier spécifique.

Principe de transmission infrarouge

Le principal problème lors d’une transmission infrarouge est lié aux perturbations de la lumière environnante. En effet, une émission directe sans codage sera extrêmement perturbé, probablement inutilisable.
Pour éviter ce problème, le signal émis en infrarouge est encodé, selon le principe suivant :

Le signal émis transmet une information codé selon le principe du code Winchester, qui transmet les informations avec une donnée de type front (montant / descendant) au lieu de logique (haut / bas).
L’amplitude du signal est très limitée : entre 889µs et 1778µs, au-delà le récepteur est saturé. Les états hauts sont également codés par une porteuse de rapport cyclique ¼, d’amplitude 7µs haut / 22µs bas.
Il est nécessaire de respecter ces contraintes pour pouvoir utiliser un récepteur intégré, qui détecte automatiquement ce type de codage et le transforme en signal logique (0/1).
L’utilisation d’un code winchester présente plusieurs avantages, mais aussi l’inconvénient d’être assez difficile à décoder. Pour simplifier, nous utiliserons (dans un premier temps) un autre moyen de distinguer un signal d’un autre : le rapport cyclique.
Entre 900 µs et 1800 µs, les amplitudes seront retransmises en l’état par le module de réception IR. Dans cette fenêtre, un rapport cyclique de 900 / 1200 pourra être distingué d’un rapport cyclique 900 / 1500 ou 1500 / 1500 etc.

Ainsi, il est possible d’émettre avec plusieurs balises d’émission selon les rapports cyclique différents, qu’un récepteur pourra identifier pour différencier les balises.

Émetteur Infrarouge

Les cartes d’émission infrarouge sont basées sur le principe ci-dessus. La version présentée ici est déployée de manière autonome dans une balise qui possède sa propre alimentation, ainsi qu’un Dip switch pour la sélection du rapport cyclique.

Étage électrique

La commande de la LED ne peut pas se faire directement par le microcontrôleur. En effet, la puissance nécessaire à l’émission de la LED est trop importante pour le microcontrôleur. Il est donc nécessaire d’ajouter un étage de puissance par l’intermédiaire d’un transistor [type BC547].

Il faut rajouter un montage pour deux LED, mais il est possible de tous les commander avec la même sortie du microcontrôleur. Les LED utilisées sont des TSAL 6200 Vishay.
Le signal Sig est associé à la variable _RB6 (en sortie), et le Dip switch est connecté aux ports _RB8 à _RB15 (en entrée).

Algorithmique

Pour l’émetteur infrarouge, les fonctions suivantes sont utilisées :

• Configuration de l’horloge : Fcy = 40 MHz, configuration 2 (cf tableau xx),
• Initialisation des timers :
  • Timer 1 : 20 ms, ainsi qu’un compteur interne de 25 pour générer un LED clignotant à 1 Hz,
  • Timer 2 : 7µs
  • Timer 3 : 9µs, ainsi qu’un compteur interne de 100 permettant une interruption à 900 µs.

Timer 1 interrupt

Le timer 4 génère un clignotement de LED à 2 Hz, utilisé comme bit de vie.

TMR1=0;

if(count>=25)

{

_RB7 = !_RB7;

count = 0;

}

else

{

count = count + 1;

}

IFS0bits.T1IF=0;

La variable count est globale, utilisée seulement ici.
Pour rappel, l’opérateur « ! » indique l’inverse d’une variable logique (ie son complément à 1). Ainsi, la fonction _RB7 = !_RB7 signifie que la variable _RB7 (port _RB7 utilisé comme E/S directe, sans passer par le registre PORTB) est mise à son complémentaire à 1 (0 si elle valait 1, 1 sinon).

Timer 2 interrupt

Le timer 2 génère la porteuse du signal infrarouge. Il déclenche une interruption toutes les 7 µs, qui est ensuite associée à un compteur à 4 pas. Une fois sur quatre, l’interruption positionne le bit_6 (commande des LED) à « x » (1 ou 0 selon l’état haut ou bas du code winchester), et trois fois sur quatre le bit_6 reste à 0. La valeur de x est générée par le timer 3.

TMR2=0;

if(count_porteuse == 1)

{

_RB6 = data_out;

count_porteuse = count_porteuse + 1;

}

else

{

_RB6 = 0;

if(count_porteuse >= 4)

{

count_porteuse = 1;

}

else

{

count_porteuse = count_porteuse + 1;

}

}

IFS0bits.T2IF=0;

La variable count_porteuse est globale, utilisée uniquement ici.
La variable data_out est globale, utilisée ici en lecture et dans timer 3 en écriture.

Timer 3 interrupt

Le timer 3 va générer l’impulsion entre 900 et 1800 µs. Pour cela, une interruption toutes les 9µs ainsi qu’un compteur >ADU<

TMR3=0;

if(elapse >= pulse)

{

data_out = !data_out;

elapse = 0;

data_in = (_RB4*2) + _RA4;

switch (data_in)

{

case 0 :

long_bit = 100;

short_bit = 200;

break;

case 1 :

long_bit = 100;

short_bit = 100;

break;

case 2 :

long_bit = 200;

short_bit = 100;

break;

case 3 :

long_bit = 150;

short_bit = 150;

break;

}

if(data_out == 1)

{

pulse = long_bit;

}

else

{

pulse = short_bit;

}

}

else

{

elapse = elapse + 1;

}

IFS0bits.T3IF=0;

Initialisation du signal

ADU

Récepteur infrarouge

Le récepteur infrarouge est calibré pour lire les signaux générés conformément à la figure [Principe de transmission infrarouge]. Il utilise principalement les détecteurs TSOP48 de Vishay (mais il en existe une foultitude) qui réalisent directement l’intégration de la porteuse pour filtrer les perturbations lumineuses. La longueur du signal détecté n’est pas exactement celle qui est émise. En effet, la commutation des fronts est réalisée lorsque plusieurs occurrences de porteuse sont détectées (ou qu’une absence de codeuse est détectée).

Étage électrique

La sortie « signal » du récepteur infrarouge est directement lisible par le µC, sans étage d’adaptation. Les 4 signaux sont câblés sur 4 ports du µC réglés en entrées (ie _RBx = 1). Le récepteur infrarouge utilise en plus l’interruption INT1 comme entrée de codeuse, sur laquelle un signal carré indiquera à chaque front montant l’avancement d’un pas du moteur pas à pas (ou d’une codeuse quelconque). L’entrée est réalisée sur le port _RB8. Une seule entrée codeuse est nécessaire ici, pour cette application les 4 récepteurs sont associés à la même motorisation.

Algorithmique

Le principe de détection est décrit dans le schéma ci-dessous :

Nota : Attention : si le ratio détecté est 4/3, c’est que la balise émet en ¾ (car inversion du signal par le récepteur).
Deux fonctions sont nécessaires :

• Détecter que l’on reçoit un message d’une balise,
• Identifier la balise.

Pour la première fonction, il est possible d’utiliser la limite de saturation du récepteur comme indication. En effet, puisqu’aucun signal ne peut transmettre des états logiques hauts supérieurs à 2 ms (en tenant compte d’une marge d’erreur, au lieu de 1778µs), si aucun changement d’état à l’entrée du détecteur n’est détecté pendant un temps supérieur à 2 ms (mettons 3 ms pour avoir une bonne marge), c’est qu’aucune balise n’est en vue. Dès lors, la limite de rollover du timer est fixée à 3 ms :

TMRiCON = 0x0010 ;

PMRi = 15000 ;

Lors d’un rollover du timer, une interruption sera générée qui permettra un traitement particulier.
L’input capture (IC) est paramétré pour détecter chaque front, montant ou descendant. En effet, il est nécessaire de mesurer la durée des fronts haut et bas pour en déduire le rapport cyclique.
A la détection d’un front montant ou descendant, une interruption IC est déclenchée et le contenu du timer (associé à l’input capture, le même que celui paramétré précédemment) est transmis au module IC. Si ce temps est le temps depuis la dernière remise à 0 du timer, réalisée au dernier front détecté, la valeur du timer est la durée du front.
Si le front est bas à l’interruption, alors la valeur du timer est la durée du front haut, sinon celle du front bas.
Une limitation de la fonction IC n’autorise l’utilisation que des timers 2 ou 3 pour la recopie de la valeur (cf Interruption Input Capture]). Pour les IC 7 et 8 (le chip dispose de 4 IC : 1, 2, 7 et 8), on utilisera les timers 4 et 5, recopiés manuellement dans la fonction d’interruption puis remis à 0.

if(_RB10 == 0)

{

count_IR[2][0] = TMR5; // Step 1

TMR5 = 0;

}

else

{

count_IR[2][1] = TMR5; // Step 0

TMR5 = 0;

[…]

}

Dès lors qu’une interruption IC est activée, le récepteur détecte une balise. La valeur de rotation de la tourelle est relevée à la première interruption IC après une interruption du timer associé (cf schéma ci-dessus, recopie de l’entrée codeuse) et le timer remis à 0 dans tous les cas.
Lorsque une interruption Timer est levée à son tour, c’est qu’aucune balise n’est en vue (pas de signal cohérent détecté). La valeur de la tourelle est relevée (entrée codeuse) à la première interruption timer après une interruption IC associée, indiquant la sortie de la fenêtre de détection. Cette fenêtre sera explicitée plus bas.

Chaîne de détection

Quatre chaines de détections sont disponibles dans l’application traitée ici. Chaque chaine dispose de fonctions spécifiques et de variables spécifiques associées à sa détection. De manière générale, les variables sont des tableaux de variables dont chaque variable est associée à une chaine. Une chaine de détection est composée :

• Du timer supervisant le rollover et cadencé pour la mesure du rapport cyclique,
• D’un Input Capture associé mesurant le rapport cyclique,
• D’une variable int detection_en_cours[4] qui indique, pour chaque chaine, si une balise est en vue ou non,
• D’une variable unsigned int cds_detect[4] qui est la valeur de la codeuse associée à la chaine,
• D’une variable int count_IR[4][2], qui est la variable contenant la longueur des fronts hauts et bas mesurés ([2] => [0] pour le front bas, [1] pour le front haut).

En plus de ces fonctions et variables, quelques fonctions génériques et variables globales sont définies :

• int calculate_angle(int count_a, int count_b), fonction de d’identification d’une balise en fonction de la longueur des fronts hauts et bas,
• traite_IR : si cette variable est à 1, cela signifie qu’une balise a été vue par l’une des chaines et qu’elle doit être identifiée,
• current_IR : indique quelle est la chaine qui a détecté une balise, pour son identification,

Enfin, diverses variables seront utilisées localement pour la réalisation de fonctions particulières (notamment mémorisation de variables), elles seront explicitées au cas par cas.

Timer 1 interrupt

La fonction est identique à celle de l’émetteur infrarouge.

Configuration d'une chaîne de détection

Les quatre chaines de détection sont identiques, avec la répartition des fonctions suivantes :

Chaine 1 => Timer 2 => IC1

Chaine 2 => Timer 3 => IC2

Chaine 3 => Timer 4 => IC7

Chaine 4 => Timer 5 => IC8

Dans la suite de ce chapitre, les références i (n° de chaine), j (numéro de timer) et k (numéro d’input capture) seront utilisées pour présenter la configuration des 4 chaines en même temps.

Initialisation du timer j :

Le timer j est initialisé pour générer une interruption toutes les 3 ms, indiquant un rollover (cf plus haut), et s’incrémente toutes les 0,2 µs.

TiCON = 0x0010 ;

PMRi = 15000 ;

Initialisation de l’input capture k :

Les quatre IC s’initialisent dans la même fonction :

ICkCON = 0x0081 ; => pour k = 1, on recopie timer 2

ICkCON = 0x0001 ; => pour k = 2,3 et 4, on recopie timer 3.

La configuration des ports se fait par les deux registres RPINR7 et RPINR10. Chacun de ces mots contrôle un port par registre (MSB / LSB) :

RPINR7 = 0x0B0A ; => L’IC1 est associée à la broche RP10 (10 = 0x0A), l’IC2 à la broche RP11 (0x0B)

RPINR10 = 0x0D0C ; => L’IC7 est associée à la broche RP12 (0x0C), l’IC8 à la broche RP13 (0x0D)

Initialisation de l’interruption externe :

L’interruption externe réalise le comptage pour les chips :

RPINR0 = 0x0900; => La broche d’entrée pour les interruptions externes est la broche RP9

_INT1EP = 0; => Les interruptions sont sur front montant.

Initialisation des ports :

Les broches associées à l’Input Capture et à l’interruption externe sont contrôlées directement par les drivers. Ils ne sont donc pas concernés par la direction des ports.

Les interruptions

Comme la plupart des applications, le récepteur IR fonctionne principalement avec les interruptions. L’algorithme de détection, pour chaque chaine, est le suivant :

1. Détection d’un changement d’état sur l’input capture alors qu’aucune détection n’est en cours (detection_en_cours[i] == 0),
2. Relevé de la position de la tourelle (ie valeur de la codeuse tourelle),
3. Identification du rapport cyclique (ie de la balise émettrice),
4. Détection d’une fin de détection (ie rollover du timer associé à la chaine) si une détection est en cours (ie detection_en_cours[i] == 1),
5. Relevé de la position de la tourelle (ie valeur de la codeuse tourelle),
6. Calcul de la position milieu de la tourelle dans sa fenêtre de détection,
7. Transmission de cette position ainsi que du numéro de balise au BF via le réseau CAN.

Chacune de ces étapes sera détaillée ci-dessous au travers de trois fonctions, associées aux chaines vues précédemment. Chaque chaine aura un timer et un input capture dédié, la fonction d’identification du rapport cyclique étant générique et appelée automatiquement dès lors qu’un récepteur est en cours de détection.   Interruption Input capture

L’interruption input capture va réaliser la détection d’un changement d’état (fonction nominale de l’input capture) et recopier la valeur courante de la codeuse si il s’agit du premier changement d’état depuis le rollover.

void __attribute__((interrupt, no_auto_psv)) _ICkInterrupt(void)

{

if(_RBn == 0)

// Si le récepteur est à l’état bas

{

count_IR[i][0] = IC1BUF;

// Durée du pulse haut

TMRj = 0;

// Réinit du timer pour une nouvelle mesure

}

Else

// Si le récepteur est à l’état haut

{

count_IR[i][1] = IC1BUF;

// Durée du pulse bas

TMRj = 0;

// Réinit du timer pour une nouvelle mesure

traite_IR = 1;

// Déclenche la fonction d’identification

current_IR = i;

// Chaine à identifier

}

A noter, pour l’input capture 1 on peut utiliser l’IC1BUF, qui est rempli automatiquement avec la valeur du timer 2 lors de la détection. Pour les autres chaines, il faudra recopier la valeur du timer associé : count_IR[i][0] = TMRj;

Afin de relever le début de la fenêtre de détection, une variable est testée à chaque interruption. Seule une interruption timer pourra remettre l’état à 0 et ainsi déclencher la réaction associée. Une fois une balise en vue, et tant qu’elle le reste (ie les interruptions IC à moins de 3 ms), la valeur de la codeuse n’est plus relevée :

if(detection_en_cours[i] == 0)

{

detection_en_cours[i] = 1;

cds_detect[i] = codeuse;

}

_IC1IF = 0;

}   Interruption timer

L’interruption timer intervient lors d’un rollover du compteur timer, soit au bout de 3 ms sans détection. Cela correspond à une zone sans balise en vue.

Donc dès lors que cette interruption intervient lorsque la variable detection_en_cours[i] est positionnée à 1, elle indique la sortie de la zone de détection.

void __attribute__((interrupt, no_auto_psv)) _TjInterrupt (void)

{

TMRj=0;

if(detection_en_cours[i] == 1)

{

Un étage de filtrage des mauvaises détections est ajouté dans la procédure. Cet étage est géré dans la boucle principale, il sera détaillé par la suite. Memory_detecteur[i] n’est supérieur à 0 qu’en cas de détection correcte.

if(memory_detecteur[i]>0)

{

Il est nécessaire de tenir compte du sens de la codeuse, pour éviter d’envoyer des angles négatifs au BF :

if(cds_detect[i] >= codeuse)

{

cds_detect[i] = cds_detect[i] - (cds_detect[i] - codeuse)/2;

}

else

{

cds_detect[i] = cds_detect[i] + (codeuse - cds_detect[i])/2;

}

Dès lors que la fenêtre de détection est identifiée, puis que son centre est calculé, les données sont transmises au BF (confer ICD pour trouver le message associé).

canTxMessage.buffer=0;

canTxMessage.id=220;

canTxMessage.data[0] = memory_detecteur[i];

canTxMessage.data[1] = GET_LOW_BYTE(cds_detect[i]);

canTxMessage.data[2] = GET_HIGH_BYTE(cds_detect[i]);

canTxMessage.data_length=3;

send_can = 1;

}

}

// On reset toutes les variables de la chaine

memory_detecteur[i] = 0;

detecteur[i] = 0;

detection_en_cours[i] = 0;

IFS0bits.TkIF=0;

}

Le traitement complet n’est réalisé que lorsqu’une fenêtre de détection est en cours. Identification d’une balise L’identification d’une balise est une fonction générique appelée dans la boucle principale. La fonction main réalise les initialisations de fonctions :

oscConfig();

init_pic();

/****/

IFS0=0;

IFS1=0;

IFS2=0;

IFS3=0;

IFS4=0;

/****/

timer1 = 1;

timer2 = 0;

timer3 = 1;

timer4 = 0;

timer5 = 1;

Ainsi que l’initialisation de la communication CAN :

/* Enable ECAN1 Interrupt */

IEC2bits.C1IE=1;

/* enable Transmit interrupt */

C1INTEbits.TBIE=1;

/* Enable Receive interrupt */

C1INTEbits.RBIE=1;

/* Enable Error interrupt */

C1INTEbits.ERRIE = 1;

canTxMessage.message_type=CAN_MSG_DATA;

canTxMessage.frame_type=CAN_FRAME_STD;

Avant de rentrer dans la boucle principale while(1) :

if(send_can == 1)

{

send_can = 0;

sendECAN(&canTxMessage);

}

Si une demande de communication est réalisée par l’un des chaines, la structure remplie est transmise. Plusieurs demandes concourantes s’écrasent, mais la rapidité du système évite (en règle générale) la perte d’information.

if(traite_IR == 1)

{

traite_IR = 0;

switch(current_IR)

{

Un switch est réalisé pour exécuter un code spécifique à chaque chaîne <<ADU : Optimiser avec une variable et un seul traitement>>

case 1 :

detecteur[0] = calculate_angle(count_IR[0][0], count_IR[0][1]);

if (detecteur[0] > 0)

{

memory_detecteur[0] = detecteur[0];

}

break;

case 2 :

detecteur[1] = calculate_angle(count_IR[1][0], count_IR[1][1]);

break;

case 3 :

detecteur[2] = calculate_angle(count_IR[2][0], count_IR[2][1]);

if (detecteur[2] > 0)

{

memory_detecteur[2] = detecteur[2];

}

break;

case 4 :

detecteur[3] = calculate_angle(count_IR[3][0], count_IR[3][1]);

break;

}

}

La fonction calculate_angle est détaillée ci-dessous, c’est elle qui identifie les balises en fonction des rapports cycliques (count_IR[i][0] pour l’état haut et count_IR[i][1] pour l’état bas). Pour rappel, le sens de détection est l’inverse de celui d’émission (le récepteur IR fonctionne en logique inversée).  

int calculate_angle(int count_a, int count_b)

{

On gère ici les différents cas possibles en fonction des mesures. La longueur de l’intervalle peut être allongée pour augmenter le nombre de balises détectables, au détriment de la précision (on augmente en effet le risque de déborder d’une durée caractéristique si cette dernière est trop courte).

if((count_a < 5000) && (count_a > 4000)) //[800 - 1000] µs

{

if((count_b < 5000) && (count_b > 4000)) //[800 - 1000] µs

{

// Balise B

return 2;

}

else if((count_b < 9500) && (count_b > 8500)) //[1700 - 1900] µs

{

// Balise C

return 3;

}

else

{

// -1 est le code erreur, la balise détectée est inconnue

return -1;

}

}

else if((count_a < 9500) && (count_a > 8500)) //[1700 - 1900] µs

{

if((count_b < 5000) && (count_b > 4000)) //[800 - 1000] µs

{

// Balise A

return 1;

}

else

{

return -1;

}

}

else if((count_a < 7250) && (count_a > 6250)) //[1250 - 1450] µs

{

if((count_b < 7250) && (count_b > 6250)) //[1250 - 1450] µs

{

// Balise D

return 4;

}

else

{

return -1;

}

}

else

{

return -1;

}

}

Il est nécessaire de recalculer (finement) toutes les valeurs de test si l’on change les types de balises (ie leur rapport cyclique). Le tableau ci-dessous résume la configuration présente (40 MHz) :

Driver moteur pas à pas