Quick and dirty arctangent

Mike Green

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.

James Anderson

math.......AAH!

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
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

Mike Green

Well, I went back to floating point and modified the assembly language Atan2 routine and converted it to Spin. This works with FloatMath, takes two integer parameters and returns an integer result rounded to the nearest 2 degrees. It's intended for use with the HM55B compass and I intend to add some calibration adjustment code eventually.

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.

JoeCreate

Thanks Mike! I came to look for HM55B stuff (in Spin) as well as an Atan2 function (in Spin) and some simple SHIFTIN and SHIFTOUT functions. And here you've provided all that in one nice little package.