' HeadTracker v11 2014_02_11 JYB (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'accelerometre
symbol Ay=w2 ' b4_b5
symbol Az=w3 ' b6_b7
Symbol Pitch=w4
Symbol Roll=w5
Symbol pan = b12
symbol Mx=w7 ' b14_b15 valeurs brutes du magnetometre
symbol My=w8 ' b16_b17
symbol Mz=w9 ' b18_b19
symbol Cx=w10 ' X Compense
symbol Cy=w11 ' Y compense
symbol Angle = w20
symbol P=w21 ' variable temporaire
symbol R=w22 ' parametre pour multiplications et divisions
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 draconiene)
' En effet, ce code est recopie autant de fois que la macro est appelee...
' 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
#MACRO Affiche(Val)
Q=Val
gosub Affiche_Sub
#ENDMACRO
#MACRO Cosinus(Val)
Q=Val
gosub Cosinus_Function
#ENDMACRO
#MACRO Sinus(Val)
Q=Val
gosub Sinus_Function
#ENDMACRO
#MACRO Multipl(Val) ' multiplie Q par R
R=Val
gosub Multipl_Function
#ENDMACRO
}
{' Initialisations
setfreq m8 'frequence
servo B.1,150
{'************ initialisation accelerometre *******************
hi2csetup i2cmaster ,%00110000, i2cfast_8, i2cbyte
hi2cout $20,($27) 'activation
hi2cout $23,($00) 'mode 2g
b24=1
}
{'************ initialisation magnetometre *******************
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cout $02,($00) 'mode conversion continu
}
pause 100
}
hi2csetup i2cmaster ,%00110000, i2cfast_8, i2cbyte
hi2cin $A8, (b2,b3,b4,b5,b6,b7) ' lecture Accelerometre
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
for i=1 to 5
hi2cin $03, (b15,b14,b19,b18,b17,b16)' lecture des valeurs magnet X , Z et Y
next i
gosub Compense
ATAN2(cX,cY)
A0=Alpha
'================================================= =====================
'|||||||||||||||||||||| Programme principal |||||||||||||||||||||||||||
'================================================= =====================
do
hi2csetup i2cmaster ,%00110000, i2cfast_8, i2cbyte
hi2cin $A8, (b2,b3,b4,b5,b6,b7) ' lecture Accelerometre
hi2csetup i2cmaster ,%00111100, i2cfast_8, i2cbyte
hi2cin $03, (b15,b14,b19,b18,b17,b16) ' lecture Magnetometre
gosub Compense
ATAN2(cX,cY)
'sertxd ("alpha=",#Alpha," A0=",#A0)
if Alpha > A0 then
bit0 = 0 'sens cw
angle = Alpha - A0
else
bit0 = 1 'sens ccw
angle = A0 - Alpha
endif
if Angle > 180 then 'on prend le supplement et on inverse le sens
angle = 360 - angle
bit0 = not bit0
endif
angle = angle max 90
Affiche(Roll)
if bit0 = 1 then
pan = angle / 2 + 150
else
angle = angle /2
pan = 150 - angle
end if
servopos B.1,pan
'sertxd (" sens: ",#bit0," angle: ",#angle," pan: ",#pan,cr)
loop
'================================================= =====================
'|||||||||||||||||||||| Programme secondaire ||||||||||||||||||||||||||
'================================================= =====================
{ ' Sous programme de calcul de cX et cY compens?s
Compense:
' X Y Z
' maxi 542 688 187
' mini -452 -408 -835
'delta 994 1096 1022
'delta/2 497 548 511
' -45 140 324
mx=mx-45 ' etalonnage
my=my-140
mz=mz+324
gosub Pitch_Roll
Cosinus(pitch) ' resultat dans Q
Multipl(mX) ' Q=Q*Mx
cX=Q
Sinus(Pitch)
Multipl(mZ)
cX=cX-Q ' Xc=Xe * cos(Pich)- Ze *sin(Pitch)
Sinus(Roll)
Multipl(mX)
cY=Q
Sinus(Pitch)
Multipl(cY)
cY=Q ' Xe * Sin(Roll) * Sin(Pitch)
Cosinus(Roll)
Multipl(mY)
cY=cY+Q ' + Ye * Cos(Roll)
Sinus(Roll)
Multipl(mZ)
P=Q
Cosinus(Pitch)
Multipl(P)
cY=cY+Q ' + Ze * Sin(Roll) * Cos(Pitch)
Return
}
{ ' Affichage des nombre sign?s
Affiche_Sub:
if q<32768 then
else
q=-q
q=-q
endif
return
}
{'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
Q=cos Alpha
Pitch=-alpha '180-Alpha
else
Alpha=-Ax/16
gosub ASIN_P
Q=Cos Alpha
Pitch=alpha '180+Alpha
endif
if Ay<32768 then
Alpha=Ay/16*100/Q
gosub ASIN_P '180+Alpha
Roll=alpha
else
Alpha=-Ay/16*100/Q
gosub ASIN_P '180-Alpha
Roll=-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
Cosinus_Function:
if Q>32767 then
q=-q
endif
Q=cos q
return
Sinus_Function:
if Q<32768 then
Q=sin q
else
q=-q
q=sin q
q=-q
endif
return
Multipl_Function: ' resultat dans Q
if Q<32768 then
if R<32768 then
Q=R/2*Q+25/50
else
R=-R
Q=R/2*Q+25/50
Q=-Q
endif
else
Q=-Q
if R<32768 then
Q=R/2*Q+25/50
Q=-Q
else
R=-R
Q=R/2*Q+25/50
endif
endif
return
}