Quick and dirty arctangent
Mike Green
Posts: 23,101
I'm starting to play with a Propellor Demo Board set on a BOE-BOT chassis with a PING bracket and HM55B compass mounted on the bracket. I seem to be having problems with downloading large Propellor programs (like with the Float32Full, Float32A, and FloatString objects), but I've come up with a little assembly routine to use the sine/cosine tables to estimate the arctangent of the -y/x value supplied by the compass. I'm sure there are all kinds of ways to improve it and I'll be working on that, but it avoids using the floating point routines. I'm attaching the archive for anyone who would like to try it or comment on it. The arctangent is monotonic over a single quadrant and I'm going to handle the signs of the sine/cosine and x/y differently so I can use a binary search over just one quadrant, then apply the signs and adjust the angle appropriately for the quadrant.
Comments
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
There once was a lady named Bright
who traveled much faster than light
She departed one day in a relative way
and Returned on the Previous Night
Cheers,
Wannabe Ub3r Geek
PRI Atan2(y,x) | xs,ys,t,i,s,tx ys := y ~> 31 xs := x ~> 31 y := f.FFloat(|| y) x := f.FFloat(|| x) if y == 0.0 if xs return 180 return 0 if x == 0.0 if ys return 270 return 90 t := f.FDiv(y,x) t := f.FDiv(f.FSub(t,1.0),f.FAdd(t,1.0)) tx := f.FMul(t,t) s := long[noparse][[/noparse]@atanTbl][noparse][[/noparse]0] repeat i from 1 to 5 s := f.FAdd(f.FMul(s,tx),long[noparse][[/noparse]@atanTbl][i]) s := f.FAdd(f.FMul(s,t),constant(pi/4.0)) s := f.FRound(f.FMul(s,constant(180.0/pi))) if ys if xs s := 180 + s s := 360 - s if xs s := 180 - s return (s + 1) & $1FE DAT atanTbl long -0.0117212, 0.05265332, -0.011643287 long 0.19354346, -0.33262348, 0.99997723 [/i]For those interested, the whole test program is attached. It includes copies of the SPI routines from the BS2 object, a modified PING routine and a simple 3 servo driver in Spin that runs in a cog.