If you need assistance, please send an email to forum at 4hv dot org. To ensure your email is not marked as spam, please include the phrase "4hv help" in the subject line. You can also find assistance via IRC, at irc.shadowworld.net, room #hvcomm.
Support 4hv.org!
Donate:
4hv.org is hosted on a dedicated server. Unfortunately, this server costs and we rely on the help of site members to keep 4hv.org running. Please consider donating. We will place your name on the thanks list and you'll be helping to keep 4hv.org alive and free for everyone. Members whose names appear in red bold have donated recently. Green bold denotes those who have recently donated to keep the server carbon neutral.
Special Thanks To:
Aaron Holmes
Aaron Wheeler
Adam Horden
Alan Scrimgeour
Andre
Andrew Haynes
Anonymous000
asabase
Austin Weil
barney
Barry
Bert Hickman
Bill Kukowski
Blitzorn
Brandon Paradelas
Bruce Bowling
BubeeMike
Byong Park
Cesiumsponge
Chris F.
Chris Hooper
Corey Worthington
Derek Woodroffe
Dalus
Dan Strother
Daniel Davis
Daniel Uhrenholt
datasheetarchive
Dave Billington
Dave Marshall
David F.
Dennis Rogers
drelectrix
Dr. John Gudenas
Dr. Spark
E.TexasTesla
eastvoltresearch
Eirik Taylor
Erik Dyakov
Erlend^SE
Finn Hammer
Firebug24k
GalliumMan
Gary Peterson
George Slade
GhostNull
Gordon Mcknight
Graham Armitage
Grant
GreySoul
Henry H
IamSmooth
In memory of Leo Powning
Jacob Cash
James Howells
James Pawson
Jeff Greenfield
Jeff Thomas
Jesse Frost
Jim Mitchell
jlr134
Joe Mastroianni
John Forcina
John Oberg
John Willcutt
Jon Newcomb
klugesmith
Leslie Wright
Lutz Hoffman
Mads Barnkob
Martin King
Mats Karlsson
Matt Gibson
Matthew Guidry
mbd
Michael D'Angelo
Mikkel
mileswaldron
mister_rf
Neil Foster
Nick de Smith
Nick Soroka
nicklenorp
Nik
Norman Stanley
Patrick Coleman
Paul Brodie
Paul Jordan
Paul Montgomery
Ped
Peter Krogen
Peter Terren
PhilGood
Richard Feldman
Robert Bush
Royce Bailey
Scott Fusare
Scott Newman
smiffy
Stella
Steven Busic
Steve Conner
Steve Jones
Steve Ward
Sulaiman
Thomas Coyle
Thomas A. Wallace
Thomas W
Timo
Torch
Ulf Jonsson
vasil
Vaxian
vladi mazzilli
wastehl
Weston
William Kim
William N.
William Stehl
Wesley Venis
The aforementioned have contributed financially to the continuing triumph of 4hv.org. They are deserving of my most heartfelt thanks.
Registered Member #14
Joined: Thu Feb 02 2006, 01:04PM
Location: Prato/italy
Posts: 383
For a windvane averaging system i had to design my own trig functions (the in-built pic basic ones suck)
It uses a quarter wave 16Bit lookup table to calculate sin cos is sin(x+90) tan is sin/cos Atan2 is useful to calculate angles from the components. Full singolarity handling and 0,1° angular resolution.
It uses about 4000 words and 130 bytes of ram
You must use:
Include "TRIG-16B.BAS"
in your picbasic
---- SIN ----- IN: T_Angle = your angle in 0...65535 format, corresponding to 0...6553.5 degrees [WORD] OUT: T_RES = your SIN, in 0...65535 format (the T_RES_SIGN takes care of sign) [WORD] T_RES_SIGN = sign of the above, 0 = POSITIVE, 1 = NEGATIVE [BIT]
---- COS ----- IN: T_Angle = your angle in 0...65535 format, corresponding to 0...6553.5 degrees [WORD] OUT: T_RES = your COS, in 0...65535 format (the T_RES_SIGN takes care of sign) [WORD] T_RES_SIGN = sign of the above, 0 = POSITIVE, 1 = NEGATIVE [BIT]
---- TAN ----- IN: T_Angle = your angle in 0...65535 format, corresponding to 0...6553.5 degrees [WORD] OUT: T_RESL = your TAN, in 0...2147483647 format (2147483647 is threated as infinity) [LONG] T_RESL_SIGN = sign of the above, 0 = POSITIVE, 1 = NEGATIVE [BIT] T_RES_ERR = overflow bit [BIT]
---- ATAN2 ----- IN: T_x, T_y = Vector components in 0...2147483647 format [LONG], the function takes care of scaling, singularities and so on OUT: T_Angle = your angle in 0...3599 format, corresponding to 0...355.9 degrees [WORD] with correct calculation of angles such 90° and 270°
Hope it will be useful
'****************************************************************
'* Name : TRIG-16B.BAS *
'* Author : Jonathan Filippi *
'* Notice : Copyright (cc) 2010 Jonathan Filippi *
'* : All Rights Reserved *
'* Date : 25/03/2010 *
'* Version : 1.0 *
'* Notes : SIN, COS and TAN and ATAN2 in 16-bit precision *
'* : *
'****************************************************************
HRSIN var word[91]
T_Angle var WORD
T_Angle2 var WORD
T_Angle3 var WORD
T_Angle4 var WORD
T_RES var WORD
T_RES_SIGN vaR BIT
T_w0 var WORD
T_w1 var word
T_w2 var word
T_w3 var word
T_s0 var byte
T_s1 var byte
T_s2 var byte
T_b0 var bit
T_b1 var bit
T_RESL VAR LONG
T_x VAR LONG
T_y VAR LONG
T_x_SIGN VAR BIT
T_y_SIGN VAR BIT
T_l0 VAR LONG
T_l1 VAR LONG
T_l2 VAR LONG
T_l0_SIGN VAR BIT
T_l1_SIGN VAR BIT
T_l2_SIGN VAR BIT
T_RES_ERR var byte
T_i var byte
SYMBOL SIGN_POSITIVE = 0
SYMBOL SIGN_NEGATIVE = 1
SYMBOL ERR_NONE = 0
SYMBOL ERR_OVERFLOW = 1
SYMBOL INFINITY = 2147483647
SYMBOL T_SCALE = 10000
'quarter wave 16 bit lookup table for SIN function, the rest is derived from this!
HRSIN[0]=0
HRSIN[1]=1144
HRSIN[2]=2287
HRSIN[3]=3430
HRSIN[4]=4571
HRSIN[5]=5712
HRSIN[6]=6850
HRSIN[7]=7987
HRSIN[8]=9121
HRSIN[9]=10252
HRSIN[10]=11380
HRSIN[11]=12505
HRSIN[12]=13625
HRSIN[13]=14742
HRSIN[14]=15854
HRSIN[15]=16962
HRSIN[16]=18064
HRSIN[17]=19161
HRSIN[18]=20251
HRSIN[19]=21336
HRSIN[20]=22414
HRSIN[21]=23486
HRSIN[22]=24550
HRSIN[23]=25607
HRSIN[24]=26655
HRSIN[25]=27696
HRSIN[26]=28729
HRSIN[27]=29752
HRSIN[28]=30767
HRSIN[29]=31771
HRSIN[30]=32768
HRSIN[31]=33753
HRSIN[32]=34728
HRSIN[33]=35693
HRSIN[34]=36647
HRSIN[35]=37589
HRSIN[36]=38521
HRSIN[37]=39440
HRSIN[38]=40347
HRSIN[39]=41243
HRSIN[40]=42125
HRSIN[41]=42995
HRSIN[42]=43851
HRSIN[43]=44695
HRSIN[44]=45524
HRSIN[45]=46340
HRSIN[46]=47142
HRSIN[47]=47929
HRSIN[48]=48701
HRSIN[49]=49460
HRSIN[50]=50203
HRSIN[51]=50930
HRSIN[52]=51642
HRSIN[53]=52339
HRSIN[54]=53019
HRSIN[55]=53683
HRSIN[56]=54331
HRSIN[57]=54962
HRSIN[58]=55577
HRSIN[59]=56174
HRSIN[60]=56755
HRSIN[61]=57318
HRSIN[62]=57864
HRSIN[63]=58392
HRSIN[64]=58902
HRSIN[65]=59395
HRSIN[66]=59869
HRSIN[67]=60325
HRSIN[68]=60763
HRSIN[69]=61182
HRSIN[70]=61583
HRSIN[71]=61965
HRSIN[72]=62327
HRSIN[73]=62671
HRSIN[74]=62996
HRSIN[75]=63302
HRSIN[76]=63588
HRSIN[77]=63855
HRSIN[78]=64103
HRSIN[79]=64331
HRSIN[80]=64539
HRSIN[81]=64728
HRSIN[82]=64897
HRSIN[83]=65047
HRSIN[84]=65176
HRSIN[85]=65286
HRSIN[86]=65375
HRSIN[87]=65445
HRSIN[88]=65495
HRSIN[89]=65525
HRSIN[90]=65535
goto skiptrig:
'******************************************************************************
'********** T_SIN, format T_Angle [WORD] -> T_RES[WORD];T_RES_SIGN [BIT]
'********** This function calculates the sin of the angle.
'********** Format of the angle: 0..3600 , corresponding to 0...360° with resolution
'********** in complementary form , for eg. -45° = 3150( 315,0°)
'********** of 0.1°, calculated by linear interpolation of points
'********** the value is in -65535...+65535 format
'******************************************************************************
T_SIN:
T_Res_ERR = ERR_NONE
T_Angle = T_Angle // 3600 'if the angle is exceeded
' determining the quadrant, appling the correct phase and then calling T_SIN_INTERP
T_s0 = T_angle / 900 ' shared variables
T_w1 = T_Angle // 900 ' shared variables
if (T_s0 = 0) then
' First quadrant, applying the function as is
T_w0 = T_w1
gosub T_SIN_INTERP:
T_RES = T_w0
T_RES_SIGN = SIGN_POSITIVE 'positive
return
endif
if (T_s0 = 1) then
' second quadrant, applying the function reversed
T_w0 = 900 - T_w1
gosub T_SIN_INTERP:
T_RES = T_w0
T_RES_SIGN = SIGN_POSITIVE 'positive
return
endif
if (T_s0 = 2) then
' third quadrant, applying the function as is but the sign is negative
T_w0 = T_w1
gosub T_SIN_INTERP:
T_RES = T_w0
T_RES_SIGN = SIGN_NEGATIVE 'negative
return
endif
if (T_s0 = 3) then
' forth quadrant, applying the function reversed and its sign is negative
T_w0 = 900 - T_w1
gosub T_SIN_INTERP:
T_RES = T_w0
T_RES_SIGN = SIGN_NEGATIVE 'negative
return
endif
return
'******************************************************************************
'********** T_COS, format T_Angle [WORD] -> T_RES[WORD];T_RES_SIGN [BIT]
'********** This function calculates the cos of the angle.
'********** Format of the angle: 0..3600 , corresponding to 0...360° with resolution
'********** in complementary form , for eg. -45° = 3150( 315,0°)
'********** of 0.1°, calculated by linear interpolation of points
'********** the value is in -65535...+65535 format
'******************************************************************************
T_COS:
'simply shift the phase of 90° and calculate the sin
T_Angle2 = T_angle
T_Angle = T_Angle + 900
Gosub T_Sin:
T_angle = T_angle2
return
'******************************************************************************
'********** T_TAN, format T_Angle [WORD] -> T_RESL[LONG];T_RES_SIGN [BIT]
'********** This function calculates the tangent of the angle.
'********** Format of the angle: 0..3600 , corresponding to 0...360° with resolution
'********** in complementary form , for eg. -45° = 3150( 315,0°)
'********** of 0.1°, calculated by linear interpolation of points
'******************************************************************************
T_TAN:
'use the functions above
T_Res_ERR = ERR_NONE
T_Angle2 = T_angle
T_Angle = T_Angle // 3600
if (T_Angle = 900) OR (T_Angle = 2700) then
T_resL = 2147483647 'Infinity
T_Res_ERR = ERR_OVERFLOW
T_s0 = T_angle / 900
T_RES_SIGN = SIGN_POSITIVE
if T_s0 = 1 then
T_RES_SIGN = SIGN_NEGATIVE
endif
if T_s0 = 3 then
T_RES_SIGN = SIGN_NEGATIVE
endif
return
endif
gosub T_Sin:
T_w2 = T_Res
T_b0 = T_RES_SIGN
GOSUB T_COS:
T_w3 = T_Res
T_b1 = T_RES_SIGN
T_RESL = T_w2*10000/T_W3
T_RES_SIGN = T_b1*T_b1
T_angle = T_angle2
return
'******************************************************************************
'********** ATAN function for quarter wave, don't use it, used internally
'********** T_l0 input variable 0..+INF
'********** In this range the function is monotone so it is easy to interpolate
'******************************************************************************
T_ATAN_QUARTER: ' iterative method for monotone functions
T_Angle4 = 4500 ' starting from 45°
T_Angle3 = 4500
T_RESL = 0
While T_Angle3 > 0
T_Angle = T_angle4 / 10
if (T_Angle4 dig 0) >=5 then
T_Angle = T_Angle + 1
endif
Gosub T_TAN:
T_Angle3 = T_Angle3 / 2
if T_l0 < T_RESL then
T_Angle4 = T_Angle4 - T_angle3
else
T_Angle4 = T_Angle4 + T_angle3
endif
WEND
return
'******************************************************************************
'********** T_ATAN2, T_x;T_y[LONG];T_x_SIGN;T_y_SIGN[BIT] -> T_Angle[WORD]
'********** This function calculates the angle corresponding for a 2D vector
'********** Format of the angle: 0..3600 , corresponding to 0...360° with resolution
'********** of 0.1°
'**********
'******************************************************************************
T_ATAN2:
' first checking for singularities
if T_y = 0 then ' Y singularity
If T_x_SIGN = SIGN_POSITIVE then
T_Angle = 0 ' if the y is zero the angle is 0° if x is positive
else
T_Angle = 1800 ' if the y is zero the angle is 180° if x is negative
endif
return
endif
if T_x = 0 then
T_l0 = INFINITY ' "infinity"
else
' scaling for best resolution
if T_y > Infinity / T_SCALE then ' T_Y is too large to scale up, scaling down the other
T_l0 = T_y / (T_x / 10000)
else
T_l0 = T_y*T_SCALE / T_x ' scaling up T_Y
endif
endif
if T_l0 > 10000000 then ' X singularity
If T_y_SIGN = SIGN_POSITIVE then
T_Angle = 900 ' if the x is zero the angle is 90° if y is positive
else
T_Angle = 2700 ' if the y is zero the angle is 270° if x is negative
endif
return
endif
Gosub T_ATAN_QUARTER: 'determining the arctangent regardless of the quadrant
if T_x_sign = SIGN_POSITIVE then
if T_y_sign = SIGN_POSITIVE then
' quadrant 1, the angle is correct as is
return
else
T_Angle = 3600 - T_Angle ' quadrant 4 the angle is negative
return
endif
else
if T_y_sign = SIGN_POSITIVE then
T_Angle = 1800 - T_Angle ' quadrant 2, the angle is complementary
return
else
T_Angle = 1800 + T_Angle ' quadrant 3, the angle is supplementary
return
endif
endif
return
'******************************************************************************
'********** Interpolation function for SIN, don't use it, used internally
'********** T_w0 input/output variable 0..900 -> 0..90°
'********** In this range the function is monotone so it is easy to interpolate
'******************************************************************************
T_SIN_INTERP:
'checking if the ancle is "round"
t_s0 = T_w0 DIG 0
T_s1 = T_w0 / 10
if (T_s0) = 0 then 'angle is round
T_w0 = HRSIN[T_s1] ' return the value
return
else 'angle is not round, interpolating
T_s2 = T_s1 + 1 ' picking the two closest points
T_w1 = (HRSIN[T_s2]-HRSIN[T_s1])*T_s0 'calculating their difference
T_w0 = T_w1 / 10 + HRSIN[T_s1] ' scaling and summing
return
endif
return
skiptrig: ' label at the end of the soubroutines
Registered Member #2099
Joined: Wed Apr 29 2009, 12:22AM
Location: Los Altos, California
Posts: 1716
Nice work, Jonathan. Here's a friendly review.
1. Well organized and well commented (better than the work of some professionals I know).
2. Based on sin and cos ranges of +-65535, I think there are a couple of errors in your sin(x) table. For 29 degrees you have 31771, should be 31772. For 48 degrees you have 48701; should be 48702.
3. You didn't specify the accuracy, but linear interpolation between points 1.0 degree apart gives sin(x) errors as large as -3 LSB's (about -45 ppm) for angles near 90 degrees, where the sin function is most strongly curved. This error is proportional to the square of the interval between points in the table, So you can improve it by a factor of four (to less than one LSB) by having lookup-table entries for every half-degree.
4. Without iterative algorithms or linear interpolation, you can get fast high-resolution sin values using the trig. identity sin(a+b) = sin a cos b + cos a sin b. e.g. for your 0.1 degree resolution, instead of a 900 point table you could have a 30 point coarse sin table (0 to 87 in steps of 3 degrees) and a 30 point fine sin and cos table (0 to 2.9 in steps of 0.1 degree). Angular resolution could increase to 0.001 degree (comparable to your sin output resolution) with tables only 10x deeper. The method can be handy in fast hardware computation, for example DDS in an FPGA.
5. The reduction of arbitrary angles to quadrant values (and fine/coarse components if desired) is more efficient if your angles are represented as binary fractions of a circle. For example, 1/4096 which is about 0.09 degrees. Many shaft angle encoders use these "natural" units of angular measurement.
Registered Member #14
Joined: Thu Feb 02 2006, 01:04PM
Location: Prato/italy
Posts: 383
Klugesmith wrote ...
Nice work, Jonathan. Here's a friendly review.
Thank you for interest :D
Klugesmith wrote ...
1. Well organized and well commented (better than the work of some professionals I know).
Wow, thank you! It is always fine to receive positive comments about my work.
Klugesmith wrote ...
2. Based on sin and cos ranges of +-65535, I think there are a couple of errors in your sin(x) table. For 29 degrees you have 31771, should be 31772. For 48 degrees you have 48701; should be 48702.
Probably rounding errors, I have corrected the table according to your values
Klugesmith wrote ...
3. You didn't specify the accuracy, but linear interpolation between points 1.0 degree apart gives sin(x) errors as large as -3 LSB's (about -45 ppm) for angles near 90 degrees, where the sin function is most strongly curved. This error is proportional to the square of the interval between points in the table, So you can improve it by a factor of four (to less than one LSB) by having lookup-table entries for every half-degree.
The reason i choose to limit to 90 elements is to make the software affordable for most PICS, even those with 16K words of program memory and 1K of ram. Also this accuracy is waay too much for the wind vane since i have angles in 10 bit resolution (using PWM output of AS5040), so i have to scale (1/64) it after multiplying the unity vector by the wind speed modulus. In fact I save the average wind angle in 0,1° resolution. In future probably i will try to extend it to make it useful for other purposes.
Klugesmith wrote ...
4. Without iterative algorithms or linear interpolation, you can get fast high-resolution sin values using the trig. identity sin(a+b) = sin a cos b + cos a sin b. e.g. for your 0.1 degree resolution, instead of a 900 point table you could have a 30 point coarse sin table (0 to 87 in steps of 3 degrees) and a 30 point fine sin and cos table (0 to 2.9 in steps of 0.1 degree). Angular resolution could increase to 0.001 degree (comparable to your sin output resolution) with tables only 10x deeper. The method can be handy in fast hardware computation, for example DDS in an FPGA.
did you mean 0.01 ° resolution?
Interesting, I didn't thought about this method it would give very high resolution trigonometry. Thank you for the input. To arrive to a similar angular resolution for ATAN2, 17 iterations are necessary, not very much more compared to the actual 13.
30 coarse and 30 sin and 30 cos fine table is 90 elements, the same of mine but with less error probably. (180Bytes ram)
to increase to 0.01° :
90 coarse and 100 sin and 100 cos , 290 not very much (580bytes ram)
0.001° is unaffordable for PICS, it would cost too much in terms of program memory and ram and interpolation would not work with 16 bit variables maybe a third table would work but we are close to the 16bit resolution
30 coarse + 30 sin and 30 cos fine table , plus 100 sin and 100 cos (repeating the identity) total , 290 (580 bytes ram) as you have stated before :D
Klugesmith wrote ...
5. The reduction of arbitrary angles to quadrant values (and fine/coarse components if desired) is more efficient if your angles are represented as binary fractions of a circle. For example, 1/4096 which is about 0.09 degrees. Many shaft angle encoders use these "natural" units of angular measurement.
You are right, the micro would be happy with his natural ability to work with numbers of this type. Humans think in decimals, electronics in binary :D
Thank you for the inputs, I will work at a second version of the library with more general use using 0....65535 angular resolution (0.005° resolution)
Registered Member #2099
Joined: Wed Apr 29 2009, 12:22AM
Location: Los Altos, California
Posts: 1716
TheMerovingian wrote ...
Klugesmith wrote ... ...Angular resolution could increase to 0.001 degree (comparable to your sin output resolution) with tables only 10x deeper
did you mean 0.01 ° resolution?
No ... with the sin(a+b) formula, table size goes up as the square root of angular resolution. For 0.001 ° resolution, instead of 90000 point table you have a 300 point coarse angle table (sin/cos for a quadrant, in steps of 0.3 degrees) and two 300 point fine angle tables (sin and cos for 0, 0.001, ..., 0.299 degrees).
Hmm - Note that we need separate fine angle tables for sin and cos, while the coarse angle table (covering a full quadrant) covers both sin AND cos. So if lookup-table ROM space is the critical cost, the 0.001 degree version is more efficiently served with a 450 point coarse table (0.2 degree intervals) and 200 point fine tables (0.199 degree range).
Checking your original 1-degree table was a trivial exercise after cutting and pasting it into a popular spreadsheet calculator program.. Can be used the other way to create source code with algorithmically generated constant data.
Registered Member #14
Joined: Thu Feb 02 2006, 01:04PM
Location: Prato/italy
Posts: 383
Another method, more intensive in terms of calculation power but less in terms of tables would be to use the taylor series for interpolation:
Sin(A+x) = SIN(A) + COS(A)*X - SIN(A) * X^2 / 2
A is the value from the table (the coarse one) and x the increment. SIN(A) is the value obtained from lookup table and COS(A) is the value of SEN(90-A) (can be obtained from the same table), also adding more polynomials would increase the precision (of course never beyond the precision of the table, whithout increasing bits of the lookup table)
The taylor series works for radians so X (in 1/10 of deg) must be converted accordingly using scaling x * 1000 / 572957
Algorithm using two long variables l0, l1,l2
L0 = SIN[A]
L1 = SIN[90-A] * X
L2 = SIN[A] * X * X * 500 / 572057
L1 = (L1 + L2) * 1000 / 572057
L0 = L0 + L1
Example:
linear interpolation for angle=89.5 gives:
65530 (the real value is 65532,504628010465407402570726936, that can be rounded to 65532 or 65533)
L0 = L0 + L1 = 65525 + 7 (7,49) = 65532 (really it should be 65532,49)
the error is -1/2LSB maximum, due to the rounding.
Using third order taylor is useless for 16-Bit table and a 32bit table makes the calculations a bit mor complicated (must take care of roll-overs for values higher than (4294967296 unsigned or 2147483648 signed).
Resolution can be increased increasing taylor polynomial but precision not, if the table is 16-Bit based, using finer angle table will not increase it very much even using identity (sin(a+b)) because the precision of the starting values of the identity is quantized at 16 bit.
Maybe adding 8 bits to the table for each element would help
but the calculation will be more complex and system intensive.
I will modify the algorithm to use the second order taylor polynomial to get rid of the -3LSB error.
Thank you
EDIT: Taylor is too much calculation intensive. I will use the sin(a+b) identity
EDIT2: THis is the last version, usin SIN(A+B) identity and some rounding bugs and minor errors.
'
**************************************************
************** '* Name : TRIG-16B.BAS * '* Author : Jonathan Filippi * '* Notice : Copyright (c) 2010 Jonathan Filippi * '* : All Rights Reserved * '* Date : 25/03/2010 * '* Version : 1.2 * '* Notes : SIN, COS and TAN and ATAN2 in 16-bit precision * '* : SUM, DIFF , signed 32bit * '* : Changes: using SIN(A+B)=SIN(A)COS(B)+COS(A)SIN(B) * '* : identity and some minor bugfixes and rounding * '* : problems, this version uses a bit less memory but * '* : is a bit slower due to the replacement of * '* : linear interpolation with true trig identity but * '* : higher precision is assured though * '* : (max error +-1/2LSB @ 16Bit due to rounding * '
**************************************************
**************
HRSINC var word[46] HRSINF var word[20] HRCOSF var word[20] T_Angle var WORD T_Angle2 var WORD T_Angle3 var WORD T_Angle4 var WORD T_RES var WORD T_RES_SIGN vaR BIT T_w0 var WORD T_w1 var word T_w2 var word T_w3 var word T_s0 var byte T_s1 var byte T_s2 var byte T_b0 var bit T_b1 var bit T_RESL VAR LONG T_x VAR LONG T_y VAR LONG T_x_SIGN VAR BIT T_y_SIGN VAR BIT T_l0 VAR LONG T_l1 VAR LONG T_l2 VAR LONG T_l0_SIGN VAR BIT T_l1_SIGN VAR BIT T_l2_SIGN VAR BIT T_RES_ERR var byte T_i var byte
SYMBOL SIGN_POSITIVE = 0 SYMBOL SIGN_NEGATIVE = 1 SYMBOL ERR_NONE = 0 SYMBOL ERR_OVERFLOW = 1 SYMBOL INFINITY = 2147483647 SYMBOL T_SCALE = 10000
'Fine 16 bit lookup table for SIN function 'Resolution 0.1 degrees, from 0.1° to 1.9° (19 values) HRSINF[0]=0 HRSINF[1]=114 HRSINF[2]
=229 HRSINF[3]=343 HRSINF[4]=458 HRSINF[5]
=572 HRSINF[6]=686 HRSINF[7]=801 HRSINF[8]
=915 HRSINF[9]=1029 HRSINF[10]=1144 HRSINF[11]
=1258 HRSINF[12]=1372 HRSINF[13]=1487 HRSINF[14]
=1601 HRSINF[15]=1716 HRSINF[16]=1830 HRSINF[17]
=1944 HRSINF[18]=2059 HRSINF[19]=2173
'Fine 16 bit lookup table for COS function 'Resolution 0.1 degrees, from 0.1° to 1.9° (19 values) HRCOSF[0]=65535 HRCOSF[1]=65535 HRCOSF[2]
=65535 HRCOSF[3]=65534 HRCOSF[4]=65533 HRCOSF[5]
=65533 HRCOSF[6]=65531 HRCOSF[7]=65530 HRCOSF[8]
=65529 HRCOSF[9]=65527 HRCOSF[10]=65525 HRCOSF[11]
=65523 HRCOSF[12]=65521 HRCOSF[13]
=65518 HRCOSF[14]=65515 HRCOSF[15]
=65513 HRCOSF[16]=65509 HRCOSF[17]
=65506 HRCOSF[18]=65503 HRCOSF[19]=65499
goto skiptrig:
'
**************************************************
**************************** '********** T_SIN, format T_Angle [WORD] -> T_RES[WORD];T_RES_SIGN [BIT] '********** This function calculates the sin of the angle. '********** Format of the angle: 0..3600 , corresponding to 0...360° with resolution '********** in complementary form , for eg. -45° = 3150( 315,0°) '********** of 0.1°, calculated by linear interpolation of points '********** the value is in -65535...+65535 format '
**************************************************
**************************** T_SIN: T_Res_ERR = ERR_NONE T_Angle = T_Angle // 3600 'if the angle is exceeded ' determining the quadrant, appling the correct phase and then calling T_SIN_INTERP T_s0 = T_angle / 900 ' shared variables T_w1 = T_Angle // 900 ' shared variables if (T_s0 = 0) then ' First quadrant, applying the function as is T_w0 = T_w1 gosub T_SIN_INTERP: T_RES = T_w0 T_RES_SIGN = SIGN_POSITIVE 'positive return endif
if (T_s0 = 1) then ' second quadrant, applying the function reversed T_w0 = 900 - T_w1 gosub T_SIN_INTERP: T_RES = T_w0 T_RES_SIGN = SIGN_POSITIVE 'positive return endif
if (T_s0 = 2) then ' third quadrant, applying the function as is but the sign is negative T_w0 = T_w1 gosub T_SIN_INTERP: T_RES = T_w0 T_RES_SIGN = SIGN_NEGATIVE 'negative return endif
if (T_s0 = 3) then ' forth quadrant, applying the function reversed and its sign is negative T_w0 = 900 - T_w1 gosub T_SIN_INTERP: T_RES = T_w0 T_RES_SIGN = SIGN_NEGATIVE 'negative return endif
return
'
**************************************************
**************************** '********** T_COS, format T_Angle [WORD] -> T_RES[WORD];T_RES_SIGN [BIT] '********** This function calculates the cos of the angle. '********** Format of the angle: 0..3600 , corresponding to 0...360° with resolution '********** in complementary form , for eg. -45° = 3150( 315,0°) '********** of 0.1°, calculated by linear interpolation of points '********** the value is in -65535...+65535 format '
**************************************************
**************************** T_COS: 'simply shift the phase of 90° and calculate the sin T_Angle2 = T_angle T_Angle = T_Angle + 900 Gosub T_Sin: T_angle = T_angle2 return
'
**************************************************
**************************** '********** T_TAN, format T_Angle [WORD] -> T_RESL[LONG];T_RES_SIGN [BIT] '********** This function calculates the tangent of the angle. '********** Format of the angle: 0..3600 , corresponding to 0...360° with resolution '********** in complementary form , for eg. -45° = 3150( 315,0°) '********** of 0.1°, calculated by linear interpolation of points '
**************************************************
**************************** T_TAN: 'use the functions above T_Res_ERR = ERR_NONE T_Angle2 = T_angle T_Angle = T_Angle // 3600
if (T_Angle = 900) OR (T_Angle = 2700) then T_resL = 2147483648 'Infinity T_Res_ERR = ERR_OVERFLOW T_s0 = T_angle / 900 T_RES_SIGN = SIGN_POSITIVE if T_s0 = 1 then T_RES_SIGN = SIGN_NEGATIVE endif if T_s0 = 3 then T_RES_SIGN = SIGN_NEGATIVE endif
'
**************************************************
**************************** '********** ATAN function for quarter wave, don't use it, used internally '********** T_l0 input variable 0..+INF '********** In this range the function is monotone so it is easy to interpolate '
**************************************************
**************************** T_ATAN_QUARTER: ' iterative method for monotone functions
T_Angle4 = 4500 ' starting from arbitrary value T_Angle3 = 4500 T_RESL = 0
While T_Angle3 > 0 ' if the increment falls to zero exit (end of iterations) T_Angle = T_angle4 / 10 if (T_Angle4 dig 0) > 5 then T_Angle = T_Angle + 1 endif Gosub T_TAN: ' calculating tangent T_RESL = T_RESL T_Angle3 = T_Angle3 / 2 if T_l0 = T_RESL then ' i don't want the algorithm to bounce uselessy ' around the solution in case of an accidental equivalence ' (it is not so rare when working with fixed point math) ' Bouncing can lead to an additional error of -+0.1°
' the tangent of the angle is the same of the searched value so found the angle ' return and save some machine computing time return else if T_l0 < T_RESL then ' the value is different, checking in which half and ' adjusting accordingly T_Angle4 = T_Angle4 - T_angle3 else T_Angle4 = T_Angle4 + T_angle3 endif endif WEND
return
'
**************************************************
**************************** '********** T_ATAN2, T_x;T_y[LONG];T_x_SIGN;T_y_SIGN[BIT] -> T_Angle[WORD] '********** This function calculates the angle corresponding for a 2D vector '********** Format of the angle: 0..3600 , corresponding to 0...360° with resolution '********** of 0.1° '********** '
**************************************************
**************************** T_ATAN2:
' first checking for singularities if T_y = 0 then ' Y singularity If T_x_SIGN = SIGN_POSITIVE then T_Angle = 0 ' if the y is zero the angle is 0° if x is positive else T_Angle = 1800 ' if the y is zero the angle is 180° if x is negative endif return endif
if T_x = 0 then T_l0 = INFINITY ' "infinity" else ' scaling for best resolution if T_y > Infinity / T_SCALE then ' T_Y is too large to scale up, scaling down the other T_l0 = T_y / (T_x / 10000) else T_l0 = T_y*T_SCALE / T_x ' scaling up T_Y endif
endif
if T_l0 > 10000000 then ' X singularity If T_y_SIGN = SIGN_POSITIVE then T_Angle = 900 ' if the x is zero the angle is 90° if y is positive else T_Angle = 2700 ' if the y is zero the angle is 270° if x is negative endif return endif
Gosub T_ATAN_QUARTER: 'determining the arctangent regardless of the quadrant
if T_x_sign = SIGN_POSITIVE then if T_y_sign = SIGN_POSITIVE then ' quadrant 1, the angle is correct as is return else T_Angle = 3600 - T_Angle ' quadrant 4 the angle is negative return endif else if T_y_sign = SIGN_POSITIVE then T_Angle = 1800 - T_Angle ' quadrant 2, the angle is complementary return else T_Angle = 1800 + T_Angle ' quadrant 3, the angle is supplementary return endif endif
return
'
**************************************************
**************************** '********** Interpolation function for SIN, don't use it, used internally '********** It uses the trigonometry identity SIN(A+B)=SIN(A)COS(B)+SIN(B)COS(A) '********** T_w0 input/output variable 0..900 -> 0..90.0° '
**************************************************
**************************** T_SIN_INTERP:
'checking if the ancle is one of the table, this speeds up the things a bit t_s0 = T_w0 // 20 T_s1 = T_w0 / 20 if (T_s0) = 0 then 'angle is one of the HRSINC table T_w0 = HRSINC[T_s1] ' return the value return endif if (T_w0) < 20 then 'angle is one of the HRSINF table T_w0 = HRSINF[T_w0] ' return the value return endif if (T_w0) > 880 then 'angle is one of the HRCOSF table (quarter complementary angle) T_w0 = HRCOSF[20-T_s0] ' return the value return endif
'angle is not in table, using identity T_l1 = HRSINC[T_s1]*HRCOSF[T_s0] + 32768 'first term of the sum plus rounding correction T_w0 = T_l1 / 65535 'scaling down to comply with the values used
T_l1 = HRSINC[45-T_s1]*HRSINF[T_s0] + 32768 'second term of the sum plus rounding correction T_w3 = T_l1 / 65535 'scaling down to comply with the values used
T_w0 = T_w0 + T_w3 ' final sum
return
'
**************************************************
**************************** '********** Sums two numbers T_l0,T_l1 [LONG] T_l0_SIGN,T_l1_SIGN [BIT] '********** --> T_l2 [LONG] T_l2_SIGN [BIT] '********** I ignored the Signed LONG to ensure compatibility with the trig functions '
**************************************************
**************************** T_SUM:
If T_l0_SIGN = T_l1_Sign then 'same sign T_l2 = T_l0 + T_l1 T_l2_SIGN = T_l0_SIGN else 'different sign If T_l0 >= T_l1 then T_l2 = T_l0 - T_l1 T_l2_SIGN = T_l0_SIGN else T_l2 = T_l1 - T_l0 T_l2_SIGN = T_l1_SIGN endif endif if T_l2 = 0 then T_l2_SIGN = SIGN_POSITIVE ' if zero the sign is positive by definition endif return
'
**************************************************
**************************** '********** SUbtracts T_l0 from T_l1 - T_l0,T_l1 [LONG] T_l0_SIGN,T_l1_SIGN [BIT] '********** --> T_l2 [LONG] T_l2_SIGN [BIT] '********** I ignored the Signed LONG to ensure compatibility with the trig functions '
**************************************************
**************************** T_DIFF: T_l1_SIGN = NOT T_l1_SIGN ' invert the sign gosub T_SUM ' and sum return
This site is powered by e107, which is released under the GNU GPL License. All work on this site, except where otherwise noted, is licensed under a Creative Commons Attribution-ShareAlike 2.5 License. By submitting any information to this site, you agree that anything submitted will be so licensed. Please read our Disclaimer and Policies page for information on your rights and responsibilities regarding this site.
Free trigonometry for PIC18**** devices
