Difference between revisions of "Projets:Perso:2013:Module de localisation"

From Electrolab
Jump to: navigation, search
(→‎Utilisation d'un quartz)
(→‎Localisation par balisage infrarouge)
 
(108 intermediate revisions by one user not shown)
Line 1: Line 1:
−
== Système de balisage infrarouge ==
+
= Localisation par balisage infrarouge =
 +
Retour à [[User:Mael]]<br />
 +
Cette page présente mes travaux actuels sur la fabrication d'un système de balisage infrarouge pour mobile.<br />
 +
Le système contient les fonctions suivantes :<br />
 +
* Emission infrarouge de 4 balises autonomes,<br />
 +
* Réception infrarouge sur une tourelle, <br />
 +
* Contrôle d'un moteur pas à pas d'entrainement de la tourelle.<br />
  
 +
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.<br /> 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.<br />
  
−
!!! N'étant pas un pro du wiki, je rédige l'ensemble des descriptifs de l'application sous Word, je les porterais ensuite ici si j'y arrive (~60 pages) !!!
+
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.<br />
  
−
Cette page présente mes travaux actuels sur la fabrication d'un système de balisage infrarouge pour 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. <br />
−
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.
+
== Système de balisage infrarouge ==
  
−
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.
+
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.<br />
−
 
+
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.<br />
−
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.  
+
[[File:Table_de_jeu_-_Localisation_IR.jpg]]
 +
<br />
 +
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.<br />
 +
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).<br />
 +
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.<br />
 +
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.<br />
 +
[[File:Table_de_jeu_-_Void_area_et_conduite.jpg]]
 +
<br />
 
----
 
----
 +
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.<br />
  
−
=== Localisation par balisage infrarouge ===
+
== 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).<br />
 +
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.<br />
 +
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.
 +
[[File:Triangulation_-_principe.jpg]]<br />
 +
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.<br />
 +
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.<br />
 +
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 :
 +
{| class="wikitable centre" width="30%"
 +
|-
 +
! scope=col | AM = (AB x Sin[a]) / Tan[θAB] + AB x Cos[a]
 +
|-
 +
|}
 +
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.<br /> 
 +
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]
 +
{| class="wikitable centre" width="30%"
 +
|-
 +
! scope=col | a = Atan[((AC / Tan[θAC]) - AB) / ((AB / Tan[θAB]) - AC)]
 +
|-
 +
|}
 +
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.<br />
 +
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.
  
−
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.
+
== Détail des applications ==
−
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 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.<br />
−
>>Table + balisage<<
+
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.<br />
 +
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. <br />
 +
Pour éviter ce problème, le signal émis en infrarouge est encodé, selon le principe suivant : <br />
 +
[[File:Code_winchester.jpg]]<br />
 +
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).<br />
 +
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. <br />
 +
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).<br />
 +
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.<br />
 +
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.<br />
 +
[[File:Rapport_cyclique.jpg]]<br />
 +
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.
  
−
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.
+
=== Émetteur Infrarouge ===
−
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).
+
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.
−
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.
+
==== Étage électrique ====
−
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.
+
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]. <br />
−
>>Table + odométrie<<
+
[[File:Alimentation_LED_IR.jpg]]<br />
 +
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.<br />
 +
Le signal Sig est associé à la variable _RB6 (en sortie), et le Dip switch est connecté aux ports _RB8 à _RB15 (en entrée).<br />
 +
[[File:DIL_Switch.jpg]]<br />
 +
==== 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.<br />
 +
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.<br />
 +
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 :<br />
 +
[[File:Detection_rapport_cyclique.jpg]]<br />
 +
<span style="color: red;">'''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).'''</span><br />
 +
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.<br />
 +
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.<br />
 +
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.<br />
 +
Si le front est bas à l’interruption, alors la valeur du timer est la durée du front haut, sinon celle du front bas.<br />
 +
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.<br />
 +
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.
  
−
== Microcontrôleur DsPIC ==
+
<u>Initialisation du timer j :</u>
  
−
Cette section est dédiée à l'utilisation des microcontrôleurs DsPic de chez Microchip.
+
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.
−
Ces chips sont majoritairement utilisés dans le système présenté ici. Il est donc nécessaire de poser un certain nombre de bases sur leur utilisation, en particulier leur mise en oeuvre et les drivers disponibles.
+
: TiCON = 0x0010 ;
−
=== Pourquoi pas en Arduino? ===
+
: PMRi = 15000 ;
−
Parce que.
+
<u>Initialisation de l’input capture k :</u>
  
−
=== Configuration générale  ===
+
Les quatre IC s’initialisent dans la même fonction :
−
Les DsPic sont de petits microcontrôleurs 16 bits très puissants que l'on peut utiliser pour tout ou presque :
+
: ICkCON = 0x0081 ; => pour k = 1, on recopie timer 2
−
Contrôle de moteur par PWM,
+
: ICkCON = 0x0001 ; => pour k = 2,3 et 4, on recopie timer 3.
−
Conversion analogique numérique 16 bits (ADC),
+
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) :
−
Acquisition de signaux numériques (Input Capture)
+
: RPINR7 = 0x0B0A ; => L’IC1 est associée à la broche RP10 (10 = 0x0A), l’IC2 à la broche RP11 (0x0B)
−
Communication CAN (natif), i2c (natif), UART (natif), SPI (natif), etc.
+
: RPINR10 = 0x0D0C ; => L’IC7 est associée à la broche RP12 (0x0C), l’IC8 à la broche RP13 (0x0D)  
−
Une foultitude de timers,
+
<u>Initialisation de l’interruption externe :</u>
−
etc.
+
  
−
Il existe plusieurs familles de DsPIC : 30F, 32, 33F, 33E. Seuls les 33F sont détaillés ici.
+
L’interruption externe réalise le comptage pour les chips :
−
Dans une même famille, tous les microcontrôleurs utilisent les drivers de la même façon, avec le même code, sous réserve qu'ils disposent du driver. En effet, dans la famille des 33F il va exister pléthore de chips différents, la différence se situant au niveau du nombre de drivers, d'E/S, de timers, de place mémoire, etc.
+
: RPINR0 = 0x0900; => La broche d’entrée pour les interruptions externes est la broche RP9
−
Dans la suite, les explications seront apportées sur un DsPIC33FJ128MC802 ou un DsPIC33FJ64GP802, dont la principale différence est un PWM pour le premier.
+
: _INT1EP = 0; => Les interruptions sont sur front montant.
−
La programmation peut se faire en C ou en assembleur. Les exemples proposés sont en C, je reviendrais par la suite sur la manière de les programmer.
+
<u>Initialisation des ports :</u>
  
−
=== Schéma de câblage ===
+
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.
−
Câbler un 33F n'est pas très compliqué, mais il existe quelques petits pièges qui peuvent vampiriser beaucoup de temps et d'énergie dans un développement. Le schéma ci-dessous reprend le câblage "générique" d'un 33F :
+
===== Les interruptions =====
−
>> Schéma de câblage chip <<
+
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 :
 +
# 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),
 +
# Relevé de la position de la tourelle (ie valeur de la codeuse tourelle),
 +
# Identification du rapport cyclique (ie de la balise émettrice),
 +
# 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),
 +
# Relevé de la position de la tourelle (ie valeur de la codeuse tourelle),
 +
# Calcul de la position milieu de la tourelle dans sa fenêtre de détection,
 +
# 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.
 +
 
 +
<u>Interruption Input capture</u>
  
−
'''!MCLR''' : La patte sert à la fois à la programmation et à la mise en marche du Chip : !MCLR = 0/NC - Chip inactif, !MCLR = VDD : Chip actif.
+
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
 +
:: }
  
−
'''PGEDi/PGECi''' : broches de programmation, elles peuvent aussi avoir d'autres application lors de l'usage du chip. Il y a plusieurs mappages des PGEC/D possibles, j'utilise ici le premier.
+
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;'''
  
−
'''VSS/AVSS''' : ground, 0 logique. La broche "A-" est normalement la branche utilisée pour faire passer de la puissance dans le chip, mais en règle général il vaut mieux tout avoir sur la même source et utiliser ensuite des composant fait pour pour les aspects puissance.
+
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;
 +
}
 +
 
 +
<u>Interruption timer</u>
  
−
'''OSC1 / OSC2''' : entrées du quartz. Voir le chapitre horloge pour le détail d'utilisation.
+
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.  
  
−
'''VDD/AVDD''' : 3,3V (+/- 10%)
+
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.
 +
<u>Identification d’une balise</u>
 +
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;
 +
:: }
 +
: }
  
−
'''VCAP''' : Cette entrée doit être reliée à la masse par un condo de 10µF, l'idéal étant un tantale polarisé (CMS ou non d'ailleurs). Sans ce condo, le chip ne marchera pas et ne pourra même pas être programmé.
+
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) :
 +
{| class="wikitable centre" width="30%"
 +
|-
 +
! scope=col colspan="2" | Fréquence
 +
! scope=col | 40 MHz
 +
! scope=col | 5 MHz
 +
|-
 +
! scope=col colspan="2" | Période
 +
! scope=col | 25 ns
 +
! scope=col | 200 ns
 +
|-
 +
| align="center" width="20%" colspan="2" |
 +
Période du timer (µs)
 +
| align="center" width="10%" |
 +
0,2
 +
| align="center" width="10%" |
 +
1,6
 +
|-
 +
| align="center" width="10%" rowspan="2" |
 +
900
 +
| align="center" width="10%" |
 +
800
 +
| align="center" width="10%" |
 +
4000
 +
| align="center" width="10%" |
 +
500
 +
|-
 +
| align="center" width="10%" |
 +
1000
 +
| align="center" width="10%" |
 +
5000
 +
| align="center" width="10%" |
 +
625
 +
|-
 +
| align="center" width="10%" rowspan="2" |
 +
1350
 +
| align="center" width="10%" |
 +
1250
 +
| align="center" width="10%" |
 +
6250
 +
| align="center" width="10%" |
 +
781,25
 +
|-
 +
| align="center" width="10%" |
 +
1450
 +
| align="center" width="10%" |
 +
7250
 +
| align="center" width="10%" |
 +
906,25
 +
|-
 +
| align="center" width="10%" rowspan="2" |
 +
1800
 +
| align="center" width="10%" |
 +
1700
 +
| align="center" width="10%" |
 +
8500
 +
| align="center" width="10%" |
 +
1062,5
 +
|-
 +
| align="center" width="10%" |
 +
1900
 +
| align="center" width="10%" |
 +
9500
 +
| align="center" width="10%" |
 +
1187,5
 +
|}
  
−
=== Horloge ===
+
=== Driver moteur pas à pas ===
−
Les DsPIC peuvent être cadencés assez haut, mais ils utilisent  pour cela une PLL interne (Phase Lock Loop).
+
−
Le principe de la PLL est détaillé sur internet, nous ne verrons ici que l'application.
+
−
Trois types d'horloges sont utilisées :
+
−
o Quartz,
+
−
o Oscillateur,
+
−
o Horloge interne.
+
−
De manière générale, il est déconseillé d'utiliser des horloges trop rapides. En effet, les signaux "carrés" sont plus mauvais à ces fréquences. Une bonne pratique est de dédier un quartz de fréquence raisonnable (8 ou 10 MHz) à chaque chip, et d'en augmenter la fréquence par une PLL. 
+
−
==== Utilisation d'un quartz ====
+
−
Les quartz sont des cristaux passifs que le microcontrôleur utilise pour générer une horloge. Le quartz est relié aux broches OSC1 et OSC2 du microcontrôleur. Les quartz utilisables sont de deux types, en fonction de leur vitesse d'horloge :
+
−
- XT : quartz jusqu'à 10 MHz,
+
−
- HS : quartz de 10 MHz à 40 MHz.
+
−
Chaque broche OSC1 et OSC2 est également relié à la masse (GND), par l'intermédiaire d'un condo de 15 µF. A noter que ça marche aussi sans le condo, mais ils sont recommandés dans la doc du chip.
+
−
'''MPLAB''' : Configure\configuration bits...
+
−
o Oscillator mode : Primary oscillator (XT, HS, EC) w/ PLL
+
−
o Internal External switch over mode : Start up device with FRC, then automatically switch to bla bla bla...
+
−
o Primary oscillator source : HS Oscillator mode (pour cause de quartz utilisé à 10 MHz)
+
−
Nota : les quartz étant passifs, ils sont dédiés à un seul chip et unique chip, on ne peut pas utiliser un quartz pour plusieurs chips.
+
−
 
+
−
==== Utilisation d'un oscillateur ====
+
−
ADU
+
−
==== Configuration de l'horloge ====
+
−
Ci-dessous un bout de code permettant de paramétrer la PLL au démarrage du chip :
+
−
 
+
−
----
+
−
 
+
−
void oscConfig(void){
+
−
/* Configure Oscillator to operate the device at 40Mhz : Fosc= Fin*M/(N1*N2), Fcy=Fosc/2 */
+
−
PLLFBD=30; /* M=32 */
+
−
CLKDIVbits.PLLPOST=0; /* N1=2 */
+
−
CLKDIVbits.PLLPRE=0; /* N2=2 */
+
−
OSCTUN=0; /* Tune FRC oscillator, if FRC is used */
+
−
/* Fosc= 10E6 * 32/(2*2)    */
+
−
/* Fosc= 320.10E6 / 4        */
+
−
/* Fosc= 80 MHz              */
+
−
/* Disable Watch Dog Timer */
+
−
RCONbits.SWDTEN=0;
+
−
/* Clock switch to incorporate PLL*/
+
−
__builtin_write_OSCCONH(0x03); // Initiate Clock Switch to Primary
+
−
// Oscillator with PLL (NOSC=0b011)
+
−
__builtin_write_OSCCONL(0x01); // Start clock switching
+
−
while (OSCCONbits.COSC != 0b011); // Wait for Clock switch to occur
+
−
/* Wait for PLL to lock */
+
−
while(OSCCONbits.LOCK!=1) {};
+
−
}
+

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.
Table de jeu - Localisation IR.jpg
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.
Table de jeu - Void area et conduite.jpg

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

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.

Triangulation - principe.jpg
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 :

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

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]
a = Atan[((AC / Tan[θAC]) - AB) / ((AB / Tan[θAB]) - AC)]

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 :
Code winchester.jpg
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.
Rapport cyclique.jpg
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].
Alimentation LED IR.jpg
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).
DIL Switch.jpg

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 :
Detection rapport cyclique.jpg
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) :

Fréquence 40 MHz 5 MHz
Période 25 ns 200 ns

Période du timer (µs)

0,2

1,6

900

800

4000

500

1000

5000

625

1350

1250

6250

781,25

1450

7250

906,25

1800

1700

8500

1062,5

1900

9500

1187,5

Driver moteur pas à pas

Retrieved from "https://wiki.electrolab.fr/index.php?title=Projets:Perso:2013:Module_de_localisation&oldid=12111"