A few years back I wrote a 16 bit fixed point trig package in Forth for a microcontroller. I used it for inverse kinematics and navigation for several hobby robots. One was a small robot arm https://youtu.be/14BX50aHDAk.

I just ported it line by line from Forth to 6502 assembly using my page zero stack macros and posted it to my GitHub repo:
https://github.com/Martin-H1/6502/blob/ ... trig16.asm

It subdivides the unit circle into 1024 binary radians which was more than enough accuracy for my uses. The sine and cosine functions being table driven seem robust. But the tangent function has its limits. I scaled the output of tangent to be useful from -80 degrees to 80 degrees, but it quickly goes out of range near the Y axis. Since robots are physical machines I avoided having the robot move in ways that provoked those values.

The asin function uses a binary search to find the corresponding angle for the sine value, and has good performance.

The atan2 function uses CORDIC with eight angle samples, it's quick, but only gets to within a few brads. The one exception is when the point has a negative X component and zero Y component, then it gives the wrong answer. But again with a robot that would be a point inside the robot and not in the work envelope.

Anyway it's MIT licensed if you find it useful. Let me know if you have improvement suggestions, or spot any bugs. Particularly in the CORDIC routine.

_________________
http://WilsonMinesCo.com/ lots of 6502 resources
The "second front page" is http://wilsonminesco.com/links.html .
What's an additional VIA among friends, anyhow?

Thanks for sharing your code Martin, and applying a permissive license!

@BigEd, you're welcome.

@Garth, thanks for the links. Tables are definitely the way to go when you have the space, as they have constant execution time coupled with higher accuracy. Fortunately even ancient computers are fast compared to physical systems, so slower algorithms like CORDIC are usually good enough.

I'm curious, when you return the sin and cos are they on the data stack as if the consumer called one after the other, or blended together in some way? The thing about tangent is that a zero or near zero in the denominator is going to risk blowing out any calculation. So you have to guard against getting near it, especially when the system is a physical system.

For inverse kinematics that means the robot is likely moving a joint in a way that is unbalanced, strained, or physically impossible. I've had a few interesting programming bugs when I didn't guard against it, and the robot behaved erratically. Fortunately I only do desktop scale robot arms so there's no risks. Industrial robots are a whole other ballgame.

I'd hope that in an industrial-grade robot, you'd use a controller capable of processing coordinates in floating-point at the required speed. It could still be a 6502, just with a 68882 FPU mapped in for the heavy lifting.

You would hope, as given the cost of those systems, the CPU cost isn't an issue. But the mathematical challenges exist independent of the implementation. Particularly because for a particular tool position there can be multiple solutions, or no solutions.

_________________
http://WilsonMinesCo.com/ lots of 6502 resources
The "second front page" is http://wilsonminesco.com/links.html .
What's an additional VIA among friends, anyhow?

Working in floating point gives you the ability to handle anomalous intermediate values without blowing out the logical complexity. That's actually what IEEE-754 was intended to standardise and enable - not floating-point itself, which was already in widespread use, but the finer points of error handling and rounding to improve reliability. Most 8-bit BASICs predate that, but there's no theoretical reason why they couldn't implement IEEE-754 in a new version.

So you can come up with the correct theoretical result, then recognise that it's outside the mechanical limits of the machine, and handle that. Or you could recognise during the intermediate calculation that some particular joint is implied to be in a non-physical state, and handle the error from there.

While that's true, my guess would be that avoiding tan, with its difficult behaviour, would also be a common tactic. Indeed, by some approaches it's just as cheap to calculate both sin and cos for the same cost, and a sincos function is provided which returns two results - just as Garth's does.

_________________
http://WilsonMinesCo.com/ lots of 6502 resources
The "second front page" is http://wilsonminesco.com/links.html .
What's an additional VIA among friends, anyhow?