The problem is solved. I found another version of trig.inc in post 13, and it works well for me. I have to put the word variables x,y into long variables before doing any calculations on them.
The problem is solved. I found another version of trig.inc in post 13, and it works well for me. I have to put the word variables x,y into long variables before doing any calculations on them.
That's a good bare bones cordic version as long as you don't mind manipulating the radians. So you could either use that and do math to your liking, or you could use TrigL , which you can find with my tilt example here: http://www.picbasic.co.uk/forum/cont...-axis-g-sensor . If you use TrigL.inc, you would probably want to add a line to replace goto main with goto overcordic
and at the end of the TrigL.inc add a line overcordic:
Keep us posted, I'd love to see your project.
Walter
Hi Walter, I have been looking at this for some time now and keep coming back to this post. I am actually attempting to do a similar project to the tilt sensor project which you have built only on a single axis. I believe I am almost there now and just need a few pointers! I am looking at your code atn2. the following is the equation i require to solve: tan-1 ((zresult - calval)/SQR ((5017 - calval)^2 - (zresult - calval)^2))
Now from reading your posts would I be right in thinking that if i do the following:
Am I right in thinking that this would convert my equation as I am not interested in the hyp or does the x vaule come into play?Code:x=0 y= ((zresult - calval)/SQR ((5017 - calval)^2 - (zresult - calval)^2)) call atn2
Hi Chris,
Well, I might have to admit that the cordic may not be the best solution for a single axis tilt sensor. It is a natural for a dual or triple axis sensor. If you just have a single axis sensor, I might suggest a lookup table. Now I'm not really good at converting mathematical equations back and forth, so I can't say for sure if you are on the right track with the one you have. It doesn't resemble the ones I have used enough for me to tell. But I can show you some that I think would work. And, perhaps yours would work too. I just can't tell from here.
Here is the app note I worked with before. As you can see, the arctan equation "theta = arctan(Ax/Az)" on page 4 is a pretty cool thing. Of course, this one requires a two axis sensor, but it keeps resolution up to about 1.3 degrees. It also fits in pretty well with are atan2 cordic.
Attachment 4533
Now for a single axis sensor, your resolution at near 90 degree angles goes down to about 9 degree accuracy. This seems to lend itself better to just a lookup table. But, if you really wanted to use the cordic, you could.
For a single axis sensor, one equation for determining angle is arcsin (x) where x is acceleration in g's . Our cordic, can't do arcsin, but it can do arctan. We can use the following equation from Wikipedia:
and solve for theta.
atan(y) = atan2(y,1)
But, in our case, we are feeding y with a value which at its peak, = 1g. So, whatever value your sensor gives for 1g, you should fill in for the "1" value. y will equal 2 arctan x/((1+sqr(1-x^2)) , then run it through the cordic. I just feel if its going to be off by up to 9 degrees (because we are using a single sensor) why not just use a lookup table.
Last edited by ScaleRobotics; - 12th February 2012 at 16:47.
http://www.scalerobotics.com
Hi,
I am using your TRIG.inc in a small program. The values appear correct from (ang) = 0 to 9000, 0 to 90.00 degrees. For values, 9000 < (ang) < 18000 ,the
values for x and y appear to switch. As an example with an (ang) value of 8900 or 89.00 degrees I receive a value back x = 29995 and y = 525. This seems correct.
For an (ang) value of 9100 or 91.00 degrees I receive back x = -523 and y = 29995. For 91.00 degrees I would expect x = 29995 and y = -523 ?
- Martin
Thanks Martin,
I'll take a look at it tonight. It should be easy enough to switch them around. With the pic16 solution, you have to work in quadrants. So that's probably where my error is.
http://www.scalerobotics.com
I think I found my error in sincos. I had 36000 where 27000 now is. This did not compensate for the 90 degree shift for radians. Let me know if this works better.
Code:sincos: 'use angle as deg.dd for example 35999 is 359.99 degrees if ang < 9001 then ang = 9000 - ang 'change degrees to radians if ang > 9000 then ang = 27000 - (36000 - ang) 'change degrees to radians ang = ang * 466 ang = div32 256 asm call sin_cos endasm return
http://www.scalerobotics.com
This is getting closer. The magnitude and sign values for the sin(ang) are all good. The magnitude term for the cos(ang) is good except the sign ,msb is incorrect for quadrants 2,3 and 4. Here is a list of what I get FOR ang = 0 to 350 STEP 1000 ;
ang = 0 sin(ang) = 5 cos(ang) = 30000
ang = 1000 sin(ang) = 5209 cos(ang) = 29544
ang = 2000 sin(ang) = 10266 cos(ang) = 28188
ang = 3000 sin(ang) = 15004 cos(ang) = 25981
ang = 4000 sin(ang) = 19287 cos(ang) = 22981
ang = 5000 sin(ang) = 22983 cos(ang) = 19285
ang = 6000 sin(ang) = 25983 cos(ang) = 14998
ang = 7000 sin(ang) = 28188 cos(ang) = 10264
ang = 8000 sin(ang) = 29544 cos(ang) = 5207
ang = 9000 sin(ang) = 30000 cos(ang) = 3
ang = 10000 sin(ang) = 29544 cos(ang) = 5207
ang = 11000 sin(ang) = 28188 cos(ang) = 10264
ang = 12000 sin(ang) = 25983 cos(ang) = 14998
ang = 13000 sin(ang) = 22983 cos(ang) = 19285
ang = 14000 sin(ang) = 19287 cos(ang) = 22981
ang = 15000 sin(ang) = 15004 cos(ang) = 25981
ang = 16000 sin(ang) = 10266 cos(ang) = 28188
ang = 17000 sin(ang) = 5209 cos(ang) = 29544
ang = 18000 sin(ang) = 5 cos(ang) = 30000
ang = 19000 sin(ang) = -5207 cos(ang) = 29544
ang = 20000 sin(ang) = -10260 cos(ang) = 28190
ang = 21000 sin(ang) = -14998 cos(ang) = 25983
ang = 22000 sin(ang) = -19283 cos(ang) = 22985
ang = 23000 sin(ang) = -22981 cos(ang) = 19287
ang = 24000 sin(ang) = -25981 cos(ang) = 15004
ang = 25000 sin(ang) = -28188 cos(ang) = 10266
ang = 26000 sin(ang) = -29544 cos(ang) = 5213
ang = 27000 sin(ang) = -30000 cos(ang) = 5
ang = 28000 sin(ang) = -29549 cos(ang) = -5195
ang = 29000 sin(ang) = -28199 cos(ang) = -10258
ang = 30000 sin(ang) = -25986 cos(ang) = -14992
ang = 31000 sin(ang) = -22987 cos(ang) = -19275
ang = 32000 sin(ang) = -19296 cos(ang) = -22976
ang = 33000 sin(ang) = -15013 cos(ang) = -25977
ang = 34000 sin(ang) = -10271 cos(ang) = -28184
ang = 35000 sin(ang) = -5216 cos(ang) = -29544
- Martin
Hmmm,
Well, I can't test it from here, but this seems like it might work. (If not, I will think harder and do some testing). Thanks for the feedback Martin.
Code:sincos: 'use angle as deg.dd for example 35999 is 359.99 degrees if ang < 9001 then ang = 9000 - ang 'change degrees to radians if ang > 9000 then ang = 45000 - ang 'change degrees to crazy radians ang = ang * 466 ang = div32 256 asm call sin_cos endasm return
http://www.scalerobotics.com
Walter,
I added three lines as shown in red. With the additions all 4 quadrants are returning correct magnitude and sign values.
See attached test file.
- Martin
;************************************************* *****************************/
i var byte BANK0
j Var byte BANK0
quad var byte BANK0
x var word bank0
y var word bank0
ang var word bank0
ang_temp var word bank0
dy var word bank0
dx var word bank0
atans var word[15] bank0
atans(0) = 16384
atans(1) = 9672
atans(2) = 5110
atans(3) = 2594
atans(4) = 1302
atans(5) = 652
atans(6) = 326
atans(7) = 163
atans(8) = 81
atans(9) = 41
atans(10) = 20
atans(11) = 10
atans(12) = 5
atans(13) = 3
atans(14) = 1
goto overcordic
atan2:
asm
call atan2_sqrt
endasm
'convert to degrees.dd y is degrees
If ang < 16384 then y = 16383 - ang
if ang > 16383 then
y = 65535 - ang
y = y + 16383 'correct 90 degrees for radian
endif
y = y * 256 'divides radians to get degrees within 57ppm
y = div32 466 'degrees.dd is y, radians is ang
return
sincos:
'use angle as deg.dd for example 35999 is 359.99 degrees
ang_temp = ang ' Store ang.
if ang < 9001 then ang = 9000 - ang
if ang > 9000 then ang = 27000 - (36000 - ang)
ang = ang * 466
ang = div32 256TRIG.basTrig_example.bas
asm
call sin_cos
endasm
if ang_temp > 9000 then y = ~y + 1 ' Perform 2's complement of y if ang > 90 for cos(ang).
return
asm
; Calculates the sine and cosine of the given angle
sin_cos:
Thanks Martin,
That works! But there is a little less math in post 49. It makes the rest of the cordic do what its supposed to for the rest of the quadrants. Thanks for pointing it out.
Here is TRIG.inc V1.5
http://www.scalerobotics.com
Walter,
Thank you for your original work and the update.
Martin, thanks for bringing up again.
I was trying to make this work last year. My results were similar to Martin's.
Assuming it was something in my code. I had to settle for pbp ATN.
Can't wait to try it out
Best regards
Walter,,
Yes the statement: IF ang > 9000 then ang = 45000 - ang, does the job. For some reason the first time I tried using it the results were crazy so I simply tried to make a patch. I must have typed in something incorrectly. Excellent job Walter ! Thanks for this piece of code.
73's de Martin
hi ScaleRobotics , i am interested in your codes posted on #51. and i have a problem.
...
ang=3000
call sincos
x=60395, y=39551
...
for all ang values i get same numbers, i dont understan what is the problem? thank you
That's weird. Maybe you can show us some of your code?
http://www.scalerobotics.com
Bookmarks