Création d'un head tracker
- Thread starter dje8269
- Start date
Il y a un bug sur la valeur de départ : ça devrait commencer à zéro...
Peut-être que la première valeur lue est foireuse ?
On peut peut-être en lire quelques-une avant de commencer, genre :
Maintenant, vous pouvez aller plus vite, pas de soucis.
Mais vous pouvez vous contenter d'un demi-tour à condition de partir à l'opposé de votre précédente direction de départ.
Si vous avez fait un demi-tour avec un départ face au Nord,
refaites en un avec le départ face au sud.
Ou alors, avec un départ face au Nord, tournez dans l'autre sens.
Peu importe qu'il y ait deux tests de un demi-tour.
L'important, c'est que toutes les directions soient testées.
Et oui c'est normal que ça change pas mal quand vous penchez, et en plus ça ne change pas de la même façon suivant la direction dans laquelle vous vous trouvez...
Comme écrit dans le programme, la boussole n'est pas compensée...
Mais il faudra un autre programme pour tester ça correctement...
Peut-être que la première valeur lue est foireuse ?
On peut peut-être en lire quelques-une avant de commencer, genre :
Code:
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cout $02,($00) 'mode conversion continu
pause 100
for i=1 to 5
hi2cin $03, (b15,b14,b19,b18,b17,b16)' lecture des valeurs magnet X , Z et Y
next i
ATAN2(X,Y)
A0=Alpha
sertxd ("A0=",#A0,13,10)Oui : faudrait tester pour être bien sur.
Maintenant, vous pouvez aller plus vite, pas de soucis.
Mais vous pouvez vous contenter d'un demi-tour à condition de partir à l'opposé de votre précédente direction de départ.
Si vous avez fait un demi-tour avec un départ face au Nord,
refaites en un avec le départ face au sud.
Ou alors, avec un départ face au Nord, tournez dans l'autre sens.
Peu importe qu'il y ait deux tests de un demi-tour.
L'important, c'est que toutes les directions soient testées.
Et oui c'est normal que ça change pas mal quand vous penchez, et en plus ça ne change pas de la même façon suivant la direction dans laquelle vous vous trouvez...
Comme écrit dans le programme, la boussole n'est pas compensée...
Mais il faudra un autre programme pour tester ça correctement...
Last edited:
Voila je me suis confectionner un truc pour pouvoir un tout complet .
Donc voici le résultat ;
en commençant dans le sens des aiguilles d'une monter
View attachment 17826
En commencant dans lesens inverse des aiguilles d'une montre
View attachment 17827
Regardez le lien
Arnaque pour vendre ?
Donc voici le résultat ;
en commençant dans le sens des aiguilles d'une monter
View attachment 17826
En commencant dans lesens inverse des aiguilles d'une montre
View attachment 17827
Oui mais elle etait vendue en tant que boussole compensée je comprends pas ....
Regardez le lien
Arnaque pour vendre ?
Les attachements ne marchent pas...
Mais l'explication est claire :
Il donne accès à toutes les données nécessaires pour le raccorder à un microcontroleur et calculer la sortie compensée.
En plus, à ce prix là, il n'y a pas de miracle...
Le titre est trompeur...
Mais l'explication est claire :
Il donne accès à toutes les données nécessaires pour le raccorder à un microcontroleur et calculer la sortie compensée.
En plus, à ce prix là, il n'y a pas de miracle...
scheiß, il y a clairement un quadrant inversé. Mais j'ai enlevé le débug...
Vous avez bien fait de vérifier !
chuis naze là : j'essayerai de reprendre demain...
Programme avec le debug :
Code:
#picaxe 20X2
#no_table
symbol I=b1
symbol Xmg=w1 ' b2_b3
symbol Ymg=w2 ' b4_b5
symbol Zmg=w3 ' b6_b7
symbol X=w7 ' b14_b15
symbol Y=w8 ' b16_b17
symbol Z=w9 ' b18_b19
symbol Xm=w10
symbol Ym=w11
symbol A0=w25
symbol Alpha=w26
symbol Q=w27
#MACRO ATAN2(dx,dy) ' r?sultat retourn? dans Alpha
Xm=dx
Ym=dy
gosub ATAN2_Function
#ENDMACRO
' ATTENTION
' Ne pas mettre beaucoup de code dans une macro, (sauf optimisation draconi?ne)
' En effet, ce code est recopi? autant de fois que la macro est appel?e...
' ici, on ne recopie que le GOSUB, pas ATAN2_Function...
#MACRO Quadrant(Base) ' r?sultat retourn? dans Alpha
Q=Base
gosub Q_Function
#ENDMACRO
' HeadTracker v02 2014_02_06 JYB (Non compensé)
'
'************ initialisation *******************
setfreq m8 'fr?quence
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cout $02,($00) 'mode conversion continu
pause 100
for i=1 to 5
hi2cin $03, (b15,b14,b19,b18,b17,b16)' lecture des valeurs magnet X , Z et Y
next i
ATAN2(X,Y)
A0=Alpha
'######################################################################
'|||||||||||||||||||||| Programme principal |||||||||||||||||||||||||||
'======================================================================
do
hi2cin $03, (b15,b14,b19,b18,b17,b16)' lecture des valeurs magnet X , Z et Y
ATAN2(X,Y)
; if Alpha>A0 then
; Alpha=Alpha-A0
; else
; Alpha=Alpha+360-A0
; endif
sertxd (#X," ",#Y," ",#Z," > " ,#Alpha,13,10)
loop
end
ATAN2_Function:
if Ym <32768 then ' Ym > 0
if Xm<32768 then ' Xm > 0
Quadrant(0)
else ' Xm < 0
Xm=-Xm
Quadrant(90)
endif
else ' Ym < 0
Ym=-Ym
if Xm<32768 then ' Xm > 0
Quadrant(270)
else ' Xm < 0
Xm=-Xm
Quadrant(180)
endif
endif
RETURN
Q_Function:
if Xm>Ym then
Alpha=100*Ym/Xm
Alpha=ATAN Alpha
Alpha=Q+Alpha
else ' Xm < Ym
Alpha=100*Xm/Ym
Alpha=ATAN Alpha
Alpha=90+Q-Alpha
endif
returnBon : rien de bien grave...
Le premier programme était volumineux mais correct.
J'ai un peu trop simplifié en insérant le sous-programme Quadrant...
Donc ce sera un poil plus lourd, mais ça devrait marcher mieux :
Je me suis imprimé le pdf : je vais maintenant tenter de compenser le "roll" et le "pitch"
Mais pour ça, il va falloir lire les 6 valeurs X,Y,Z et Xm, Ym , Zm
Vu que ça va être assez calculatoire, ce serait bien d'augmenter la vitesse du processeur en changeant aussi les inits et de vérifier qu'il communique toujours sur le bus I2C...
A bientôt.
Le premier programme était volumineux mais correct.
J'ai un peu trop simplifié en insérant le sous-programme Quadrant...
Donc ce sera un poil plus lourd, mais ça devrait marcher mieux :
Code:
' HeadTracker v07 2014_02_03 JYB (Non compense)
'
#picaxe 20X2
#no_table
symbol I=b1
symbol Xmg=w1 ' b2_b3
symbol Ymg=w2 ' b4_b5
symbol Zmg=w3 ' b6_b7
symbol X=w7 ' b14_b15
symbol Y=w8 ' b16_b17
symbol Z=w9 ' b18_b19
symbol Xm=w10
symbol Ym=w11
symbol A0=w25
symbol Alpha=w26
symbol Q=w27
#MACRO ATAN2(dx,dy) ' resultat retourne dans Alpha
Xm=dx
Ym=dy
gosub ATAN2_Function
#ENDMACRO
' ATTENTION
' Ne pas mettre beaucoup de code dans une macro, (sauf optimisation draconi?ne)
' En effet, ce code est recopi? autant de fois que la macro est appel?e...
' ici, on ne recopie que le GOSUB, pas ATAN2_Function...
#MACRO Quadrant(B1,B2) ' Result ==> Alpha
if Xm>Ym then
Alpha=100*Ym/Xm
Alpha=ATAN Alpha
Alpha=B1
else ' Xm < Ym
Alpha=100*Xm/Ym
Alpha=ATAN Alpha
Alpha=B2
endif
#ENDMACRO
'************ initialisation *******************
setfreq m8 'frequence
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cout $02,($00) 'mode conversion continu
pause 100
for i=1 to 5
hi2cin $03, (b15,b14,b19,b18,b17,b16)' lecture des valeurs magnet X , Z et Y
next i
ATAN2(X,Y)
A0=Alpha
sertxd ("A0=",#A0,13,10,13,10)
'######################################################################
'|||||||||||||||||||||| Programme principal |||||||||||||||||||||||||||
'======================================================================
do
hi2cin $03, (b15,b14,b19,b18,b17,b16)' lecture des valeurs magnet X , Z et Y
ATAN2(X,Y)
if Alpha>=A0 then
Alpha=Alpha-A0
else
Alpha=Alpha+360-A0
endif
sertxd (#X," ",#Y," ",#Z," > " ,#Alpha,13,10)
loop
end
' Input Xm & Ym are 2 complement encoded
' Xm and YM can be anything from -32767 to + 32767
' So you can use Xm=X1-X2
' even if X2 greater than X1
'
ATAN2_Function:
if Ym <32768 then ' Ym > 0
if Xm<32768 then ' Xm > 0
Quadrant(Alpha,90-Alpha)
else ' Xm < 0
Xm=-Xm
Quadrant(180-Alpha,90+Alpha)
endif
else ' Ym < 0
Ym=-Ym
if Xm<32768 then ' Xm > 0
Quadrant(360-Alpha,270+Alpha)
else ' Xm < 0
Xm=-Xm
Quadrant(180+Alpha,270-Alpha)
endif
endif
RETURNMais pour ça, il va falloir lire les 6 valeurs X,Y,Z et Xm, Ym , Zm
Vu que ça va être assez calculatoire, ce serait bien d'augmenter la vitesse du processeur en changeant aussi les inits et de vérifier qu'il communique toujours sur le bus I2C...
A bientôt.
bonsoir,
Désolé pour le retard , je m'occupais du petit bout , du manger et du dodo ; ma femme attend le deuxieme alors j'aide : lol .
Je fais le test tout de suite ;
la com sur le bus i2c ne pourras pas encore être tester , car il faudrait que je l'intègre avec un connecteur dans la télécommande , et ca j'y ai que peu réfléchis encore ; Vu la taille de cette boussole , je pourrais en qu'a de problème lui dédié un picaxe tout seul et le mettre en esclave sur l'i2C sans soucis . l'idéale serait d'integrer les calcul dans mon picaxe option dans la télécommande . mais dans les deux cas ça ne devrait pas poser trop de problème .
Le seul hic on je suis incapable de vous répondre, c'est est que la longueur du cable ne sera pas trop longue pour transmettre en i2C un cable allant du dessus des sourcils , a une télécoommande dans la main , longeant le bras pour ne pas gêné ,disons 80 cm environ.
Je parle , je parle, mais allez je file essayer votre programme totu de suite ... en croisant les doigts
Désolé pour le retard , je m'occupais du petit bout , du manger et du dodo ; ma femme attend le deuxieme alors j'aide : lol .
Je fais le test tout de suite ;
Pas de soucis vous me dites et je fais .
Pour le moment la boussole est sur plaque d'essai , avec son seul 20X2 . on peut augmenter la vitesse sans problème . et ainsi reglé et bien comprendre le fonctionnement, car en l'état je ne pourrais pas présenter encore le head tracker ! . C'est le roll qui pose probléme , il faudrait reussis à le diminuer voir le supprimer , mais une diminution de l'effet serait déjà trés bien .
la com sur le bus i2c ne pourras pas encore être tester , car il faudrait que je l'intègre avec un connecteur dans la télécommande , et ca j'y ai que peu réfléchis encore ; Vu la taille de cette boussole , je pourrais en qu'a de problème lui dédié un picaxe tout seul et le mettre en esclave sur l'i2C sans soucis . l'idéale serait d'integrer les calcul dans mon picaxe option dans la télécommande . mais dans les deux cas ça ne devrait pas poser trop de problème .
Le seul hic on je suis incapable de vous répondre, c'est est que la longueur du cable ne sera pas trop longue pour transmettre en i2C un cable allant du dessus des sourcils , a une télécoommande dans la main , longeant le bras pour ne pas gêné ,disons 80 cm environ.
Je parle , je parle, mais allez je file essayer votre programme totu de suite ... en croisant les doigts
N'hesitez a me dire si ca ne va pas hein !!!!
Les conditions : 1 tour complet ( 360° ) dans le sens horaire , une pause pour bien le caler , et hop un autre tour complet dans l'autre sens ( anti-horaire).
Voila le résultat:
View attachment test BESQUEUT 5.txt
Les conditions : 1 tour complet ( 360° ) dans le sens horaire , une pause pour bien le caler , et hop un autre tour complet dans l'autre sens ( anti-horaire).
Voila le résultat:
View attachment test BESQUEUT 5.txt
Je vais regarder le test 5
En attendant, voici une V8 pour lire simultanément Accéléromètre et Magnétomètre...
Regardez déjà si Pitch & Roll correspondent à vos mouvements...
Le Heading doit correspondre quand on est à plat.
Pour info :
A chaque fois que vous avez le signe {
vous pouvez cliquer dans le petit carré à gauche pour masquer/afficher le bout de code correspondant...
Très pratique pour avoir une lecture claire du programme.
En attendant, voici une V8 pour lire simultanément Accéléromètre et Magnétomètre...
Code:
' HeadTracker v08 2014_02_03 JYB (Non compense)
'
' Conventions de nommage suivant AN3192 STMicroelectronic
'
#picaxe 20X2
#no_table
{'Définition des symboles
symbol I=b1
symbol Ax=w1 ' b2_b3 valeurs brutes de l'accéléromètre
symbol Ay=w2 ' b4_b5
symbol Az=w3 ' b6_b7
Symbol Pitch=w4
Symbol Roll=w5
symbol Mx=w7 ' b14_b15 valeurs brutes du magnétomètre
symbol My=w8 ' b16_b17
symbol Mz=w9 ' b18_b19
symbol Xm=w23 ' paramètres de ATAN2
symbol Ym=w24
symbol A0=w25 ' Angle de référence pour le Heading
symbol Alpha=w26 ' retour de ATAN2
symbol Q=w27 ' variable temporaire pour ATAN2 et autres
}
{'Définition des macros
#MACRO ATAN2(dx,dy) ' resultat retourne dans Alpha
Xm=dx
Ym=dy
gosub ATAN2_Function
#ENDMACRO
' ATTENTION
' Ne pas mettre beaucoup de code dans une macro, (sauf optimisation draconi?ne)
' En effet, ce code est recopi? autant de fois que la macro est appel?e...
' ici, on ne recopie que le GOSUB, pas ATAN2_Function...
#MACRO Quadrant(B1,B2) ' Result ==> Alpha
if Xm>Ym then
Alpha=100*Ym/Xm
Alpha=ATAN Alpha
Alpha=B1
else ' Xm < Ym
Alpha=100*Xm/Ym
Alpha=ATAN Alpha
Alpha=B2
endif
#ENDMACRO
}
{' Initialisations
setfreq m8 'frequence
{'************ initialisation accéléromètre *******************
hi2csetup i2cmaster ,%00110000, i2cfast_8, i2cbyte
hi2cout $20,($27) 'activation
hi2cout $23,($00) 'mode 2g
}
{'************ initialisation magnétomètre *******************
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cout $02,($00) 'mode conversion continu
}
pause 100
}
for i=1 to 5
hi2cin $03, (b15,b14,b19,b18,b17,b16)' lecture des valeurs magnet X , Z et Y
next i
ATAN2(Mx,My)
A0=Alpha
sertxd (13,10,"A0=",#A0,13,10)
sertxd ("Ax Ay Az Mx My Mz > Pitch Roll Heading",13,10)
'======================================================================
'|||||||||||||||||||||| Programme principal |||||||||||||||||||||||||||
'======================================================================
do
hi2csetup i2cmaster ,%00110000, i2cfast_8, i2cbyte
hi2cin $A8, (b2,b3,b4,b5,b6,b7) ' lecture Accéléromètre
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cin $03, (b15,b14,b19,b18,b17,b16) ' lecture Magnétomètre
atan2(Ax,Az)
Pitch=Alpha
atan2(Ay,Az)
Roll=Alpha
ATAN2(Mx,My)
if Alpha>=A0 then
Alpha=Alpha-A0
else
Alpha=Alpha+360-A0
endif
sertxd (#Ax," ",#Ay," ",#Az," ",#Mx," ",#My," ",#Mz," > " ,#Pitch," ",#Roll," ",#Alpha,13,10)
loop
end
{'ATAN2
ATAN2_Function:
' Input Xm & Ym are 2 complement encoded
' Xm and YM can be anything from -32767 to + 32767
' So you can use Xm=X1-X2
' even if X2 greater than X1
'
if Ym <32768 then ' Ym > 0
if Xm<32768 then ' Xm > 0
Quadrant(Alpha,90-Alpha)
else ' Xm < 0
Xm=-Xm
Quadrant(180-Alpha,90+Alpha)
endif
else ' Ym < 0
Ym=-Ym
if Xm<32768 then ' Xm > 0
Quadrant(360-Alpha,270+Alpha)
else ' Xm < 0
Xm=-Xm
Quadrant(180+Alpha,270-Alpha)
endif
endif
RETURN
}Le Heading doit correspondre quand on est à plat.
Pour info :
A chaque fois que vous avez le signe {
vous pouvez cliquer dans le petit carré à gauche pour masquer/afficher le bout de code correspondant...
Très pratique pour avoir une lecture claire du programme.
Ben ça me parait pas mal, et c'est surtout à vous que ça doit plaire !
Il y a eu un petit accroc vers la fin du premier tour vers 100°
mais ça semble plausible vu vos conditions d'expérimentation.
Par ailleurs, on retrouve la position de départ à 1° près ce qui est remarquable
tant pour la précision de vos manipulations que pour la stabilité du magnétomètre.
Voulez vous que je passe en setfreq m32 ?
POur le test de la V8 . si j'ai bien compris; les trois valeurs derrière le signe ">" doivent correspondent respectivement aux pitch roll et heading .
Si c'est bien le cas , seul la valeur du heading est correct . les autres valeurs restent toutes les deux a 269-270 , et quand j'incline vraiment fort par exemple pour le pitch( hochement de tete) la valeur du roll passe a 180 environ . et quand j'incline fort le roll (oreille sur les épaules) c'est la valeur de pitch qui passe a 180° .
Voulez vous que je fasse des tests pour chaque axe ? je les mets a la suite dans .txt avec une séparation ?
POur le test de la V8 . si j'ai bien compris; les trois valeurs derrière le signe ">" doivent correspondent respectivement aux pitch roll et heading .
Si c'est bien le cas , seul la valeur du heading est correct . les autres valeurs restent toutes les deux a 269-270 , et quand j'incline vraiment fort par exemple pour le pitch( hochement de tete) la valeur du roll passe a 180 environ . et quand j'incline fort le roll (oreille sur les épaules) c'est la valeur de pitch qui passe a 180° .
Voulez vous que je fasse des tests pour chaque axe ? je les mets a la suite dans .txt avec une séparation ?
Trop fort , effectivement lors du premier tour complet , j'ai eu du mal a finir en un seul mouvement j'ai donc fais un accoup pour remplacer ma main ! .
Je m'aligne avec le bord de la table , c'est apparemment assez efficace . j'ai fixé la plaque sur un livre de papier glacé pour pouvoir bien pivoter a plat . et fixer le tout avec du schtoch . plutôt archaïque mais efficace
effectivement la courbe est magnifique , on retrouve aussi un tout petit accoup faire la fin quand j'essayais dele mettre pile poil comme il faut . C'est genial ca ..... Ca avance fort notre affaire !
Maintenant tout mettre en ordre et c'est génial . Si ca fonctionne je ne manquerai pas de vous faire une vidéo !.
Aujourd'hui j'ai bien bossé siur le hard du VHl . la carcasse va partir a la peinture , j'ai finis le typon final , je devrais le sortir demain ;
j'ai encore celui de la télécommande refaire en intégrant des nouveaux inter . un pour activer ou non le head-tracking, et un autre pour activer ou non le back non le back up et reflechir a un connecteur pour le head tracker mais je sors du sujet la ! .
Oui pour setfreq m32
Vu vos indications, je crois savoir ce qui ne va pas, mais c'est pas facile à corriger : les calculs intermédiaires sont trop grands.
Le problème, c'est d'arriver à travailler avec des nombres négatifs en faisant comme si ils étaient positifs...
A 32 ca bug a l'affichage , j'ai donc mis en 16Mhz .
Voici un test de pitch ; j'incline la plaque en avant de 90° puis je reviens a plat , et je la re-incline de 90° a l'arriére .
View attachment test BESQUEUT 6.txt
Voici un test de pitch ; j'incline la plaque en avant de 90° puis je reviens a plat , et je la re-incline de 90° a l'arriére .
View attachment test BESQUEUT 6.txt
V9 avec mise à l'échelle automatique avant calcul de ATAN
Votre test est très bien.
Si ça marche, idem en penchant sur le coté.
Code:
' HeadTracker v09 2014_02_03 JYB (Non compense)
'
' Conventions de nommage suivant AN3192 STMicroelectronic
'
#picaxe 20X2
#no_table
{'Definition des symboles
symbol I=b1
symbol Ax=w1 ' b2_b3 valeurs brutes de l'acc?l?rom?tre
symbol Ay=w2 ' b4_b5
symbol Az=w3 ' b6_b7
Symbol Pitch=w4
Symbol Roll=w5
symbol Mx=w7 ' b14_b15 valeurs brutes du magn?tom?tre
symbol My=w8 ' b16_b17
symbol Mz=w9 ' b18_b19
symbol Xm=w23 ' param?tres de ATAN2
symbol Ym=w24
symbol A0=w25 ' Angle de r?f?rence pour le Heading
symbol Alpha=w26 ' retour de ATAN2
symbol Q=w27 ' variable temporaire pour ATAN2 et autres
}
{'Definition des macros
#MACRO ATAN2(dx,dy) ' resultat retourne dans Alpha
Xm=dx
Ym=dy
gosub ATAN2_Function
#ENDMACRO
' ATTENTION
' Ne pas mettre beaucoup de code dans une macro, (sauf optimisation draconi?ne)
' En effet, ce code est recopi? autant de fois que la macro est appel?e...
' ici, on ne recopie que le GOSUB, pas ATAN2_Function...
#MACRO Quadrant(Qxm,Qym) ' Result ==> Alpha
if Xm>Ym then
gosub ATAN_Xm
Alpha=Qxm
else ' Xm < Ym
gosub ATAN_Ym
Alpha=Qym
endif
#ENDMACRO
}
{' Initialisations
setfreq m8 'frequence
{'************ initialisation acc?l?rom?tre *******************
hi2csetup i2cmaster ,%00110000, i2cfast_8, i2cbyte
hi2cout $20,($27) 'activation
hi2cout $23,($00) 'mode 2g
}
{'************ initialisation magn?tom?tre *******************
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cout $02,($00) 'mode conversion continu
}
pause 100
}
for i=1 to 5
hi2cin $03, (b15,b14,b19,b18,b17,b16)' lecture des valeurs magnet X , Z et Y
next i
ATAN2(Mx,My)
A0=Alpha
sertxd (13,10,"A0=",#A0,13,10)
sertxd ("Ax Ay Az Mx My Mz > Pitch Roll Heading",13,10)
'======================================================================
'|||||||||||||||||||||| Programme principal |||||||||||||||||||||||||||
'======================================================================
do
hi2csetup i2cmaster ,%00110000, i2cfast_8, i2cbyte
hi2cin $A8, (b2,b3,b4,b5,b6,b7) ' lecture Acc?l?rom?tre
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cin $03, (b15,b14,b19,b18,b17,b16) ' lecture Magn?tom?tre
atan2(Ax,Az)
Pitch=Alpha
atan2(Ay,Az)
Roll=Alpha
ATAN2(Mx,My)
if Alpha>=A0 then
Alpha=Alpha-A0
else
Alpha=Alpha+360-A0
endif
sertxd (#Ax," ",#Ay," ",#Az," ",#Mx," ",#My," ",#Mz," > " ,#Pitch," ",#Roll," ",#Alpha,13,10)
loop
end
{'ATAN2
ATAN2_Function:
' Input Xm & Ym are 2 complement encoded
' Xm and YM can be anything from -32767 to + 32767
' So you can use Xm=X1-X2
' even if X2 greater than X1
'
if Ym <32768 then ' Ym > 0
if Xm<32768 then ' Xm > 0
Quadrant(Alpha,90-Alpha)
else ' Xm < 0
Xm=-Xm
Quadrant(180-Alpha,90+Alpha)
endif
else ' Ym < 0
Ym=-Ym
if Xm<32768 then ' Xm > 0
Quadrant(360-Alpha,270+Alpha)
else ' Xm < 0
Xm=-Xm
Quadrant(180+Alpha,270-Alpha)
endif
endif
RETURN
}
{ 'Réduction des nombres trop grands et calcul de l'arc Tangente
ATAN_Xm:
select case Ym ' Ym < Xm
case < 655 ' Ym < 655
Alpha=100*Ym/Xm
case < 6553 ' 655 < Ym < 6553 donc Xm > 655
Q=Xm/10
Alpha=10*Ym/Q
else ' 6553 < Ym donc Xm > 6553
Q=Xm/100
Alpha=Ym/Q
end select
Alpha=ATAN Alpha
RETURN
ATAN_Ym:
select case Xm ' Xm < Ym
case < 655 ' Xm < 655
Alpha=100*Xm/Ym
case < 6553 ' 655 < Xm < 6553
Q=Ym/10
Alpha=10*Xm/Q
else ' 6553 < Xm
Q=Ym/100
Alpha=Xm/Q
end select
Alpha=ATAN Alpha
RETURN
}Si ça marche, idem en penchant sur le coté.
Ah bon §!!! . pourtant les valeurs sont bizarres non ?
Je test tout de suite la V9
Je vais faire un triple test ;
Un tour complet dans les deux sens ;
Pitch aller retour a 90°
Et roll avec aller retour a 90°
Et voila , les trois sont séparés pour bien les différencier . Vous me dites ce que ca donne !
View attachment test BESQUEUT 7.txt
Last edited:
Bon : déjà je suppose que pour vous l'avant est du coté de la prise de programmation,
c'est à dire en haut du bouquin quand on peut lire le titre. C'est bien ça ?
Si c'est confirmé, d'après la photo, l'écriture SPARKFUN est dans le même sens?
Mais comme ils sont FUNs, ils ont soudé le CMS à angle droit...
Ca devrait expliquer que Pitch et Roll sont intervertis...
c'est à dire en haut du bouquin quand on peut lire le titre. C'est bien ça ?
Si c'est confirmé, d'après la photo, l'écriture SPARKFUN est dans le même sens?
Mais comme ils sont FUNs, ils ont soudé le CMS à angle droit...
Ca devrait expliquer que Pitch et Roll sont intervertis...
Non non . Pour moi l'avant c'est le côté opposé au bord de la table . C'EST à dire le côté de la prise d'alimentation.
C'est peu être moi qui est mis les connecteurs à l'envers. Je me souviens avoir hésité lors de la soudure et je me suis dis qu'il était préférable de ne pas voir les composants
Idem
Ça semble pas débile,
mais il y a une interférence entre le pitch et le roll quand on penche beaucoup.
Erreur de calcul ou problème inhérent au capteur ?
Ensuite on verra pour la compensation.
La formule est à peu près digérable, mais il faut bien trouver le zéro et le sens de chaque axe.
Avec 6 axes, une chance sur deux de se tromper pour le sens à chaque fois, et 3 chances sur 4 pour le zéro
il n'y a pas de place au hasard... (1 chance sur 48 de tomber juste...)
Les calculs ne sont pas bons : c'est pour ça que les graphiques ont une forme bizarre (sauf le heading : là c'est à cause du manque de compensation)
Je reprends ça ce soir.
J'ai laissé des valeurs supérieures à 360° pour la lisibilité du graphique.
365° ça existe et c'est la même chose que 5°
Le problème du zéro des angles, c'est de savoir s'il est en haut, en bas, en avant ou en arrière... (4 possibilités)
C'est dit sur la notice, mais fo pas se vautrer dans les calculs... Et avec une arithmétique uniquement positive, les risques d'erreur sont nombreux...
Ah oui ok , je comprends mieux .
Pas de probléme pour ce soir ! Je serais au RDV 'est la moidnre des choses .
Je viens de refaire le typon de la télécommande en incluant un connecteur 4 broches ( 2 pour l'alim et 2 pour l'i2C) , ce pour vous dire a quel point je crois en ce projet !
Comme d'hab vous me dites et je fais, j'ai essayé de me plonger dans les calculs, amis honnêtement c'est du haut niveau , je perdrais plus de temps à essayer de comprendre qu'autre choses .
V10 avec un nouveau calcul de Roll et Pitch : je croise les doigts 
Code:
' HeadTracker v10 2014_02_04 JYB (Non compense)
'
' Conventions de nommage suivant AN3192 STMicroelectronic
'
#picaxe 20X2
#no_table
{'Definition des symboles
symbol I=b1
symbol Ax=w1 ' b2_b3 valeurs brutes de l'acc?l?rom?tre
symbol Ay=w2 ' b4_b5
symbol Az=w3 ' b6_b7
Symbol Pitch=w4
Symbol Roll=w5
symbol Mx=w7 ' b14_b15 valeurs brutes du magn?tom?tre
symbol My=w8 ' b16_b17
symbol Mz=w9 ' b18_b19
symbol Xm=w23 ' parametres de ATAN2
symbol Ym=w24
symbol A0=w25 ' Angle de reference pour le Heading
symbol Alpha=w26 ' retour de ATAN2 et de ASIN
symbol Q=w27 ' variable temporaire pour ATAN2 et autres
}
{'Definition des macros
#MACRO ATAN2(dx,dy) ' resultat retourne dans Alpha
Xm=dx
Ym=dy
gosub ATAN2_Function
#ENDMACRO
' ATTENTION
' Ne pas mettre beaucoup de code dans une macro, (sauf optimisation draconi?ne)
' En effet, ce code est recopi? autant de fois que la macro est appel?e...
' ici, on ne recopie que le GOSUB, pas ATAN2_Function...
#MACRO Quadrant(Qxm,Qym) ' Result ==> Alpha
if Xm>Ym then
gosub ATAN_Xm
Alpha=Qxm
else ' Xm < Ym
gosub ATAN_Ym
Alpha=Qym
endif
#ENDMACRO
}
{' Initialisations
setfreq m8 'frequence
{'************ initialisation acc?l?rom?tre *******************
hi2csetup i2cmaster ,%00110000, i2cfast_8, i2cbyte
hi2cout $20,($27) 'activation
hi2cout $23,($00) 'mode 2g
}
{'************ initialisation magn?tom?tre *******************
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cout $02,($00) 'mode conversion continu
}
pause 100
}
for i=1 to 5
hi2cin $03, (b15,b14,b19,b18,b17,b16)' lecture des valeurs magnet X , Z et Y
next i
ATAN2(Mx,My)
A0=Alpha
sertxd (13,10,"A0=",#A0,13,10)
sertxd ("Ax Ay Az Mx My Mz > Pitch Roll Heading",13,10)
'======================================================================
'|||||||||||||||||||||| Programme principal |||||||||||||||||||||||||||
'======================================================================
do
hi2csetup i2cmaster ,%00110000, i2cfast_8, i2cbyte
hi2cin $A8, (b2,b3,b4,b5,b6,b7) ' lecture Acc?l?rom?tre
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cin $03, (b15,b14,b19,b18,b17,b16) ' lecture Magn?tom?tre
gosub Pitch_Roll
ATAN2(Mx,My)
if Alpha>=A0 then
Alpha=Alpha-A0
else
Alpha=Alpha+360-A0
endif
sertxd (#Ax," ",#Ay," ",#Az," ",#Mx," ",#My," ",#Mz," > " ,#Pitch," ",#Roll," ",#Alpha,13,10)
loop
end
{'ATAN2
ATAN2_Function:
' Input Xm & Ym are 2 complement encoded
' Xm and YM can be anything from -32767 to + 32767
' So you can use Xm=X1-X2
' even if X2 greater than X1
'
if Ym <32768 then ' Ym > 0
if Xm<32768 then ' Xm > 0
Quadrant(Alpha,90-Alpha)
else ' Xm < 0
Xm=-Xm
Quadrant(180-Alpha,90+Alpha)
endif
else ' Ym < 0
Ym=-Ym
if Xm<32768 then ' Xm > 0
Quadrant(360-Alpha,270+Alpha)
else ' Xm < 0
Xm=-Xm
Quadrant(180+Alpha,270-Alpha)
endif
endif
RETURN
}
{ 'Reduction des nombres trop grands et calcul de l'arc Tangente
ATAN_Xm:
select case Ym ' Ym < Xm
case < 655 ' Ym < 655
Alpha=100*Ym/Xm
case < 6553 ' 655 < Ym < 6553 donc Xm > 655
Q=Xm/10
Alpha=10*Ym/Q
else ' 6553 < Ym donc Xm > 6553
Q=Xm/100
Alpha=Ym/Q
end select
Alpha=ATAN Alpha
RETURN
ATAN_Ym:
select case Xm ' Xm < Ym
case < 655 ' Xm < 655
Alpha=100*Xm/Ym
case < 6553 ' 655 < Xm < 6553
Q=Ym/10
Alpha=10*Xm/Q
else ' 6553 < Xm
Q=Ym/100
Alpha=Xm/Q
end select
Alpha=ATAN Alpha
RETURN
}
{' Calcule Pitch et Roll
Pitch_Roll:
if Ax<32768 then
Alpha=Ax/16
gosub ASIN_P
Pitch=180-Alpha
else
Alpha=-Ax/16
gosub ASIN_P
Pitch=180+Alpha
endif
' Q=cos Alpha ' pas implemente.
if Ay<32768 then
Alpha=Ay/16
gosub ASIN_P
Roll=180+Alpha
else
Alpha=-Ay/16
gosub ASIN_P
Roll=180-Alpha
endif
return
ASIN_P: ' sin(90°)=1050
select case Alpha
case <750:Alpha=Alpha+8/17
case <960:Alpha=Alpha-750/10+45
case <1040:Alpha=Alpha-960/4+66
else:Alpha=90
endselect
return
}Ben c'est mieux non ?
Remarquez que les angles ne sont plus limités à 50° mais vont jusqu'à 90°
Evidemment, quand vous allez au-delà ça bloque à 90°.
Je travaille à valider la formule pour le heading sous Excel.
Pour le moment la compensation est pire que le brut...
Ben très honnêtement , je ne sais pas trop , cette nuée de chiffre me parle pas vraiment ; vos graphiques me parle déjà plus ! . Sans traitement Excel c'est pas évident . je vois bien le heading , qui me semble parfait ; sauf peut être quand on penche . mais de la a vous dire si c'est mieux , c'est impossible .
Tout repose entre vos mains , moi j'execute ! . et je vous en remercie d'ailleurs !
Si c'est plus clair pour vous, n'affichez que les 3 dernières valeurs (celles après le signe >)
Gardez la ligne sous forme de commentaire au cas où il faudrait la remettre pour debug.
La valeur 180 correspond à l'horizontale.
Si je partais de zéro, j'aurais des valeurs négatives, ce qui n'est pas possible avec un Picaxe...
De là ça grandit ou ça diminue suivant le sens de manœuvre (donc de 90° à 270°)
Est-ce clair ?
Ah oui , la c'est beaucoup plus clair.
MAis vous parlez pour le roll et le pitch ? Car le heading lui varie bien sur 360 degré?
Je ne comprends pas et je pense jamais, comment vous allez calculez la compensation. Je pense même pas que ça vaille le coup que vous essayié de m'expliquer lol. C'est juste impressionnant!
MAis vous parlez pour le roll et le pitch ? Car le heading lui varie bien sur 360 degré?
Je ne comprends pas et je pense jamais, comment vous allez calculez la compensation. Je pense même pas que ça vaille le coup que vous essayié de m'expliquer lol. C'est juste impressionnant!
Oui le heading : 360° à partir de la position de départ.
Pour la compensation : c'est pas gagné...La formule n'est pas hyper compliquée, mais il doit y avoir une erreur de signe quelque part...
Même sous Excel je n'y arrive pas. Fais faire dodo. Ça ira peut-être mieux demain...
Je viens de trouver une AN cypress
avec des formules qui se ressemblent, mais surtout une simplification très intéressante.
Ça devrait permettre de vérifier les formules de STM et d'essayer une nouvelle approche "optimisée pour un calcul sur microcontrôleur"
Ça tombe bien : c'est ça qu'on veut faire...
Egalement, une implémentation sur Arduino
qui m'a mis le doigt sur un point crucial : il faut d'abord étalonner le compas !
Bêtement, je pensait que c'était fait en usine et que ça ne servait que pour améliorer la précision.
Vu les valeurs données en exemple, ça semble tout à fait indispensable !
Ça tombe bien : votre premier test (un tour à l'horizontale" correspond pile à cet étalonnage.
Donc j'ai déjà les mesures pour faire ce travail... YAPUKA
Une autre sur arduino
Encore une variante chez circuitcellar/
Vais tester chaque méthode et choisir la meilleure... ça risque de prendre un certain temps...
avec des formules qui se ressemblent, mais surtout une simplification très intéressante.
Ça devrait permettre de vérifier les formules de STM et d'essayer une nouvelle approche "optimisée pour un calcul sur microcontrôleur"
Ça tombe bien : c'est ça qu'on veut faire...
Egalement, une implémentation sur Arduino
qui m'a mis le doigt sur un point crucial : il faut d'abord étalonner le compas !
Bêtement, je pensait que c'était fait en usine et que ça ne servait que pour améliorer la précision.
Vu les valeurs données en exemple, ça semble tout à fait indispensable !
Ça tombe bien : votre premier test (un tour à l'horizontale" correspond pile à cet étalonnage.
Donc j'ai déjà les mesures pour faire ce travail... YAPUKA
Une autre sur arduino
Encore une variante chez circuitcellar/
Vais tester chaque méthode et choisir la meilleure... ça risque de prendre un certain temps...
Last edited:
Je vous rassure : la démo des formules d'Einstein pour la relativité restreinte, je comprends strictement rien non plus.
Pour la relativité générale, c'est encore pire (si c'est possible...)
Par contre, en trigo spatiale, j'ai quelques notions... (mais c'est chronophage...)
Etalonnage du magnétomètre
J'ai des résultats encourageants, mais la boussole n'est toujours pas compensée.
Pour étalonner j'aurais besoin de la manip suivante :
- remettre la ligne de debug avec les 6 valeurs
- faire un enregistrement dans toutes les directions possibles à main levée (sans trop secouer quand même...)
==> un tour complet horizontal
==> idem en pitch et en roll
idem en tournant sur vous même de 30° en 30° environ,
jusqu'à faire un tour complet.
A chaque fois, vous retournez la crêpe dans les deux sens et en pitch et en roll.
Peu importe que ça soit précis, et vous pouvez tourner relativement vite et irrégulièrement.
Tout à la suite, sans séparation ni indication des différentes manipulations...
Le but est d'avoir les valeurs mini et maxi du magnétomètre dans toutes les directions possibles.
J'ai des résultats encourageants, mais la boussole n'est toujours pas compensée.
Pour étalonner j'aurais besoin de la manip suivante :
- remettre la ligne de debug avec les 6 valeurs
- faire un enregistrement dans toutes les directions possibles à main levée (sans trop secouer quand même...)
==> un tour complet horizontal
==> idem en pitch et en roll
idem en tournant sur vous même de 30° en 30° environ,
jusqu'à faire un tour complet.
A chaque fois, vous retournez la crêpe dans les deux sens et en pitch et en roll.
Peu importe que ça soit précis, et vous pouvez tourner relativement vite et irrégulièrement.
Tout à la suite, sans séparation ni indication des différentes manipulations...
Le but est d'avoir les valeurs mini et maxi du magnétomètre dans toutes les directions possibles.
Je vais essayer de vous faire ca , ce soir , mais je ne peux pas le garantir . Je vous le signalerais des que je commence a le faire .
Alors pour être sur que j'ai bien compris .
Alors pour être sur que j'ai bien compris .
Ca, pas de soucis
Ca c'est comme d'hab , a plat sur ma table et sur 360°
j'ai toujours fais sur 90 ° , il faut que je fasse 360 ° aussi ?
Ca avec les cables ca va pas etre possible , mais le faire tourner a plat sur la table ca reviens au même, non ?
Au sens litterale la crepe ? c'est dire que je fais un roll et pitch sur 180 ° ? en même temps que je tourne a plat , ou apres avoir tourner a plat ?
ok
Oui 180° dans les 2 sens et dans chaque direction.
Vous tenez la carte à la main, pouce dessous, les câbles coté du coude, face vers le haut.
Rotation du poignet en souplesse, le pouce se retrouve dessus.
Maintenez la carte avec la main gauche. Lâchez main droite et remettez le pouce dessous==> deuxième demi-tour.
30° sur vos talons (à vue hein : ça n'a pas besoin d'être précis!)
On refait pareil, mais dans l'autre sens histoire que les fils ne s'entortillent pas.
30° sur les talons
etc...
Pas grave si vous ne parvenez pas à faire un tour complet, mais il faut dépasser largement le demi-tour.
Çà parait loufoque, mais (à part les robots spécifiques du CERN construits pour ça...)
je n'ai pas trouvé d'autre méthode pour aligner le magnétomètre en 3D.
Les valeurs déjà fournies couvrent une bonne partie des possibilités, mais il reste des tas de cas de figure non testées (en particulier quand le capteur est à l'envers) et je pense que ça biaise pas mal les mesures.
Ce serait mieux d'être sur.
Le but c'est d'avoir le milieu entre le mini et le maxi.
Si on a les valeurs mini, mais pas le maxi (parce que cette position n'est jamais utilisée),
alors le milieu n'est pas à mi-chemin et toutes les mesures sont faussées d'autant...
Pour le moment, j'ai ça :
X Y Z
513 615 184 maxi
-450 -415 -547 mini
On voit en particulier que Zmax devrait être bien plus grand.
Si je m'en tiens à ces mesures, la valeur zéro en Y n'est pas 0 mais 100 !
Ça fait un sacré décalage.
Vous tenez la carte à la main, pouce dessous, les câbles coté du coude, face vers le haut.
Rotation du poignet en souplesse, le pouce se retrouve dessus.
Maintenez la carte avec la main gauche. Lâchez main droite et remettez le pouce dessous==> deuxième demi-tour.
30° sur vos talons (à vue hein : ça n'a pas besoin d'être précis!)
On refait pareil, mais dans l'autre sens histoire que les fils ne s'entortillent pas.
30° sur les talons
etc...
Pas grave si vous ne parvenez pas à faire un tour complet, mais il faut dépasser largement le demi-tour.
Çà parait loufoque, mais (à part les robots spécifiques du CERN construits pour ça...)
je n'ai pas trouvé d'autre méthode pour aligner le magnétomètre en 3D.
Les valeurs déjà fournies couvrent une bonne partie des possibilités, mais il reste des tas de cas de figure non testées (en particulier quand le capteur est à l'envers) et je pense que ça biaise pas mal les mesures.
Ce serait mieux d'être sur.
Le but c'est d'avoir le milieu entre le mini et le maxi.
Si on a les valeurs mini, mais pas le maxi (parce que cette position n'est jamais utilisée),
alors le milieu n'est pas à mi-chemin et toutes les mesures sont faussées d'autant...
Pour le moment, j'ai ça :
X Y Z
513 615 184 maxi
-450 -415 -547 mini
On voit en particulier que Zmax devrait être bien plus grand.
Si je m'en tiens à ces mesures, la valeur zéro en Y n'est pas 0 mais 100 !
Ça fait un sacré décalage.
Last edited: