I needed a fast simple mathematical algorithm to run on a PIC and generate a result in "degrees" as an integer 0-360 degrees, from an input which is an XY vector (which generally represents a movement). I designed this algorithm out of combining 2 imperfect systems, the combined result is fast and accurate enough for many simple tasks.

The XY vector can have positive or negative values for both X and Y, and one value can be zero (but not both). A big advantage is automatic scaling of the X and Y values that can be in a very large range.

1. They require large, slow ArcTan() or ArcCos() functions, or look-up tables

2. They have problems if a value is zero

3. They can't do more than 180 degrees

4. They usually require an additional ratio calc to be done first

5. They are very difficult to implement on a small PIC

1. It is very small and fast compared to trig math!

2. It is even smaller if you just need one quadrant 0-90 degrees or 0-45 degrees

3. It only requires 16bit multiply/divide in most cases (no 32bit math)

4. It has a simple integer output; 0-360 degrees

5. It works with any X and Y values, +/- and one value can be zero

6. It automatically avoids a divide by zero error, as long as either X or Y is non-zero

1. Accuracy! Its accuracy is +/- 1 degree worst case, but usually better

2. Its output is 0-360 degrees, not 0-359 (that is easily fixed if needed)

3. X and Y must be between -1456 and 1456 (for 16bit version)

A big benefit of this algorithm is its flexibility, being able to use any X and Y values. If you use a 32bit multiply and divide which is a little slower than the 16bit multiply and divide it is possible to use very large X and Y values. This makes it ideal for robot navigation or CNC machine coordinates where you just give it the XY values from the last move and it returns the direction as a 0-360 degree heading.

The algorithm is also ideal for processing the output of an X Y accelerometer, and providing a tilt angle in degrees. This can be done even with a very small cheap PIC, a 10F or 12F series.

This process requires a 16bit*8bit multiply, followed by a 16bit/16bit divide. This is fast and code efficient even on a small PIC.

This process only requires simple comparisons of a 1byte value (0-90 degrees) and some simple additions.

This only requires a few bit tests and a 16bit addition or subtraction.

```
```*// Fast XY vector to integer degree algorithm - Jan 2011 www.RomanBlack.com*
*// Converts any XY values including 0 to a degree value that should be*
*// within +/- 1 degree of the accurate value without needing*
*// large slow trig functions like ArcTan() or ArcCos().*
*// NOTE! X value must be greater than zero!*
*// This is the simplest version, for one octant (half a quadrant)*
*// so X must be >= Y, although any values of X and Y are usable*
*// provided they are under 1456 so the 16bit multiply does not overflow.*
*// the result will be 0-45 degrees range*
**unsigned char** tempdegree;
**unsigned char** comp;
**unsigned int** degree; *// this will hold the result*
**signed int** x; *// these hold the XY vector at the start*
**signed int** y; *//*
*// 1. Calc the scaled "degrees"*
degree = (y * **45**) / x; *// note! X must be >= Y, result will be 0-45*
*// 2. Compensate for the 4 degree error curve*
comp = **0**;
tempdegree = degree; *// use an unsigned char for speed!*
**if**(tempdegree > **22**) *// if top half of range*
{
**if**(tempdegree <= **44**) comp++;
**if**(tempdegree <= **41**) comp++;
**if**(tempdegree <= **37**) comp++;
**if**(tempdegree <= **32**) comp++; *// max is 4 degrees compensated*
}
**else** *// else is lower half of range*
{
**if**(tempdegree >= **2**) comp++;
**if**(tempdegree >= **6**) comp++;
**if**(tempdegree >= **10**) comp++;
**if**(tempdegree >= **15**) comp++; *// max is 4 degrees compensated*
}
degree += comp; *// degree is now accurate to +/- 1 degree!*

```
```*// Fast XY vector to integer degree algorithm - Jan 2011 www.RomanBlack.com*
*// Converts any XY values including 0 to a degree value that should be*
*// within +/- 1 degree of the accurate value without needing*
*// large slow trig functions like ArcTan() or ArcCos().*
*// NOTE! at least one of the X or Y values must be non-zero!*
*// This is the full version, for all 4 quadrants and will generate*
*// the angle in integer degrees from 0-360.*
*// Any values of X and Y are usable including negative values provided*
*// they are between -1456 and 1456 so the 16bit multiply does not overflow.*
**unsigned char** negflag;
**unsigned char** tempdegree;
**unsigned char** comp;
**unsigned int** degree; *// this will hold the result*
**signed int** x; *// these hold the XY vector at the start*
**signed int** y; *// (and they will be destroyed)*
**unsigned int** ux;
**unsigned int** uy;
*// Save the sign flags then remove signs and get XY as unsigned ints*
negflag = **0**;
**if**(x < **0**)
{
negflag += **0x01**; *// x flag bit*
x = (**0** - x); *// is now +*
}
ux = x; *// copy to unsigned var before multiply*
**if**(y < **0**)
{
negflag += **0x02**; *// y flag bit*
y = (**0** - y); *// is now +*
}
uy = y; *// copy to unsigned var before multiply*
*// 1. Calc the scaled "degrees"*
**if**(ux > uy)
{
degree = (uy * **45**) / ux; *// degree result will be 0-45 range*
negflag += **0x10**; *// octant flag bit*
}
**else**
{
degree = (ux * **45**) / uy; *// degree result will be 0-45 range*
}
*// 2. Compensate for the 4 degree error curve*
comp = **0**;
tempdegree = degree; *// use an unsigned char for speed!*
**if**(tempdegree > **22**) *// if top half of range*
{
**if**(tempdegree <= **44**) comp++;
**if**(tempdegree <= **41**) comp++;
**if**(tempdegree <= **37**) comp++;
**if**(tempdegree <= **32**) comp++; *// max is 4 degrees compensated*
}
**else** *// else is lower half of range*
{
**if**(tempdegree >= **2**) comp++;
**if**(tempdegree >= **6**) comp++;
**if**(tempdegree >= **10**) comp++;
**if**(tempdegree >= **15**) comp++; *// max is 4 degrees compensated*
}
degree += comp; *// degree is now accurate to +/- 1 degree!*
*// Invert degree if it was X>Y octant, makes 0-45 into 90-45*
**if**(negflag &** 0x10**) degree = (**90** - degree);
*// 3. Degree is now 0-90 range for this quadrant,*
*// need to invert it for whichever quadrant it was in*
**if**(negflag & **0x02**) *// if -Y*
{
**if**(negflag & **0x01**) *// if -Y -X*
degree = (**180** + degree);
**else** *// else is -Y +X*
degree = (**180** - degree);
}
**else** *// else is +Y*
{
**if**(negflag & **0x01**) *// if +Y -X*
degree = (**360** - degree);
}

The code above compiled on a PIC12F675 and used only 196 ROM, with an extra 29 and 43 ROM needed for the 16bit divide and multiply routines.

It is still quite usable on a small PIC as it is about 200 ROM (but also requires 32 multiply and divide functions). Using 32bit it can now accept +/- XY values anywhere in the range -95443717 to 95443717 and still return a simple integer 0-360 degrees.

```
```*// For 32bit version substitute these 5 variables below instead of the vars shown above;*
**unsigned long** degree; *// this will hold the result*
**signed long** x; *// these hold the XY vector at the start*
**signed long** y; *// (and their sign will be destroyed)*
**unsigned long** ux;
**unsigned long** uy;

The algorithm can have an imprtant optimisation to reduce ROM by removing the multiply and divide functions. If using the 32bit version this can save about 350 to 400 ROM (on a PIC16) by removing the 32bit multiply and 32bit divide functions.

This will be slower to execute but the nature of the algorithm makes this not as bad as would be expected. This is because the multiply is guaranteed to be 45 additions, and the divide is guaranteed to be between 0 and 45 subtractions for any possible values of X and Y. See code below.

```
```*// To remove the 32bit multiply and divide functions and save lots*
*// of ROM replace this mult/divide line;*
degree = (uy * **45**) / ux; *// degree result will be 0-45 range*
*// With this successive addition and subtraction code;*
*// (Which takes a maximum of; 45 additions, 0-45 subtractions)*
**unsigned char** i;
*// do uy*45 by doing 45 additions*
degree = **0**;
i = **45**;
**while**(i)
{
degree += uy;
i--;
}
*// do degree/ux by doing from 0 to 45 subtractions as needed*
i=**0**;
**while**(degree >= ux)
{
degree -= ux;
i++; *// count number of divisions*
}
degree = i; *// load result back into degree*

This algorithm and code examples on this page are open-source, use them as you like but please mention www.RomanBlack.com.

- end -

[Back to Home Page]