I’ve been working on a AL5B robotic arm. I noticed that there is a bug in the current IDE concerning FACOS in the range 0.61 to 0.68 and 0.92 to 0.94. This forced me to use a table for the acrcos function. Which is faster anyway 8)
The table represents the shape of the function from 0 to 1. The function translates the cos input to the correct table position to get the correct shape-value. This shape value is a multiplier to calculate the correct angle in radials.
The function uses values with 4 decimals. The biggest accuracy error is 0.00308 rad = 0.17 degrees. More then enough for my servos
Here is a small demo program prints the range to the serial port:
IF Index < 1000 THEN
Index = Index+10
ELSE
Index = -1000
ENDIF
pause 100
goto main
;====================================================================
;[GETARCCOS] Get the sinus and cosinus from the angle +/- multiple circles
;Cos4 - Input Cosinus
;AngleRad4 - Output Angle in AngleRad4
GetArcCos[Cos4]
Cos4 = (Cos4 max c4DEC) min -c4DEC
IF Cos4>=0 THEN
AngleRad4 = GetACos(Cos4/78+1) ;78=table resolution (1/127)
AngleRad4 = AngleRad462 ;62=acos resolution (pi/2/255)
ELSE
AngleRad4 = GetACos(-Cos4/78+1)
AngleRad4 = 31416-AngleRad462 ;62=acos resolution (pi/2/255)
ENDIF
return AngleRad4[/code]
Xan
EDIT: Improved function can be found in one of the newer posts. Please read whole topic.
Great work with the fixed point FACOS function! Have you tried replacing the FACOS in the Phoenix code (LegIK) yet? Would be great to get rid of that little bug there too. Especially the 0.61 - 0.68 was a bit irritating.
Nope, I didn’t had the time to give it a try but it shouldn’t be to hard in V2.0. I think I’ll try it this week because it made a real difference on the arm. The arm moves really steady now so this also will be a major improvement on the phoenix!
I took a quick peek at Zenta’s PEP to answer that one
“This is the angle of the line S>W with respect to the humerus, expressed in radiansâ€
No problem. This reminds me that I need to update my commends in the code.
The kit is really neat! Very solid. I changed the setup by dropping the SSC and only use a BB2 with BAP. This made one of the servo wires to short so I needed a extra extension wire. I had to get myself some of these (forgot there name again… ). The tube needs a really tight fit to make it solid and I only had some metric keys.
I’m planning to start a topic on this including some pics and vids. I hope to do it later on this week.
Nope, just the tool itself the Hex keys. So simple, jet so hard to remember. The name is totally different in dutch that’s why I keep forgetting it. LOL
The ulna is made from a aluminium tube and got a bolts to connect it to the hub.
Just tried a random value, acos(0.4512). I got an error of 0.57 deg. Instead of lookup table, perhaps can add a linear interpolation to refine the lookup. What do you think?
This is a great sample code. I could never understand what Laurent was doing with his wonderful powerpod code… (I’m very slow when it comes to reading other people’s codes )
I thought I was the only one experiencing the problem with the FACOS error in that range. I finally got fed up with why my legs had a sudden “jerk” when commanding it to trace a straight line using my IK engine…
After many s_out commands and a table I generated with a spreadsheet (OpenOffice Calc), I noticed that the numbers started to deviate from what it’s actually supposed to be!!!
I finally ended up just limiting the range of FACOS to stay away from that region… But with the table, you can use the full range AND have it scaled to actual integers!!! Not to mention a look up table is super fast (at the cost of memory of course)…
I’ve been meaning to getting rid of all my FCOS and FSIN functions as they are costly, but in the end, the Atom Pro 28 is a miniature little workhorse, I was surprised that it can keep up with the calculations even when I cranked the transit time of the servos to 100msec…
Sorry I kinda lost track of this post that’s why I didn’t end the previous one… sry for that
I’ve been experimenting a lot with this method since I’ve got both the Arm and the phoenix running on byte tables for sin/cos, acos and tan2. I found out that the error isn’t as beautiful as I thought it was so you could be right with the found error. The error is (was ) even bigger near cos=1. Interpolation should improve it a bit but it will also cost some time. I haven’t tried it but it sure is word a try.
LOL, Nope you’re one of many who are struggling with the FACOS functions. It took me some time to figure out a good balance between speed, memory and accuracy.
I’ve worked this out in excel. It’s much easier to verify stuff when seeing it on the monitor with a decent graph.
Yepp the byte table cost some memory space but the pro got plenty for our programs. Speed is the thing
Phoenix V2.0 is at 50ms cycle time at the moment
Like I said I found out that the error got much bigger near cos = 1. This is because the function is logarithmic which is giving a pretty vertical line near cos = 1. To solve this I’ve splitted the table up in to 3 parts.
Cos = 0 – 0.9
Cos = 0.9 – 0.99
Cos = 0.99 – 1
Here is the improved function:
[code]
;====================================================================
c1DEC con 10
c2DEC con 100
c4DEC con 10000
c6DEC con 1000000
;--------------------------------------------------------------------
;[TABLES]
;ArcCosinus Table
;Table build in to 3 part to get higher accuracy near cos = 1.
;The biggest error is near cos = 1 and has a biggest value of 3*0.012098rad = 0.521 deg.
;- Cos 0 to 0.9 is done by steps of 0.0079 rad. (1/127)
;- Cos 0.9 to 0.99 is done by steps of 0.0008 rad (0.1/127)
;- Cos 0.99 to 1 is done by step of 0.0002 rad (0.01/64)
;Since the tables are overlapping the full range of 127+127+64 is not necessary. Total bytes: 277
GetACos bytetable 255,254,252,251,250,249,247,246,245,243,242,241,240,238,237,236,234,233,232,231,229,228,227,225, |
224,223,221,220,219,217,216,215,214,212,211,210,208,207,206,204,203,201,200,199,197,196,195,193, |
192,190,189,188,186,185,183,182,181,179,178,176,175,173,172,170,169,167,166,164,163,161,160,158, |
157,155,154,152,150,149,147,146,144,142,141,139,137,135,134,132,130,128,127,125,123,121,119,117, |
115,113,111,109,107,105,103,101,98,96,94,92,89,87,84,81,79,76,73,73,73,72,72,72,71,71,71,70,70, |
70,70,69,69,69,68,68,68,67,67,67,66,66,66,65,65,65,64,64,64,63,63,63,62,62,62,61,61,61,60,60,59, |
59,59,58,58,58,57,57,57,56,56,55,55,55,54,54,53,53,53,52,52,51,51,51,50,50,49,49,48,48,47,47,47, |
46,46,45,45,44,44,43,43,42,42,41,41,40,40,39,39,38,37,37,36,36,35,34,34,33,33,32,31,31,30,29,28, |
28,27,26,25,24,23,23,23,23,22,22,22,22,21,21,21,21,20,20,20,19,19,19,19,18,18,18,17,17,17,17,16, |
16,16,15,15,15,14,14,13,13,13,12,12,11,11,10,10,9,9,8,7,6,6,5,3,0
;GetSinCos
cos4 var sword ;Input Cosinus of the given Angle, decimals = 4
AngleRad4 var sword ;Output Angle in radials, decimals = 4
Index var sword
Index=-1000
;====================================================================
main:
IF Index < 1000 THEN
Index = Index+10
ELSE
Index = -1000
ENDIF
pause 100
goto main
;====================================================================
;[GETARCCOS] Get the sinus and cosinus from the angle +/- multiple circles
;Cos4 - Input Cosinus
;AngleRad4 - Output Angle in AngleRad4
GetArcCos[Cos4]
;Check for negative value
IF (Cos4<0) THEN
Cos4 = -Cos4
NegativeValue = TRUE
ELSE
NegativeValue = FALSE
ENDIF
;Limit Cos4 to his maximal value
Cos4 = (Cos4 max c4DEC)
IF (Cos4>=0 AND Cos4<9000) THEN
AngleRad4 = GetACos(Cos4/79) ;79=table resolution (1/127)
AngleRad4 = AngleRad4*616/c1DEC ;616=acos resolution (pi/2/255)
[/code]** this bytetable and getAcos function are copy passed from the phoenix code. It’s possible that the code doesn’t compile at once.**
). The tube needs a really tight fit to make it solid and I only had some metric keys.
