# Angle from X,Y

How do you find the angle from (X, Y)? And how do you optimize this for realtime on the FRC? And One Of Astronouth’s Rare Contensts: Why would I care? Angle from (x,y) (I’m assuming on a normal Cartesian Plane) to what in relation to what as a vertex?

ie. angle between (x,y) and x-axis with origin as vertex.

The angle between (0,0) and (x,y) in relation to the x axis (A horizantal line, y=0)

The inverse tangent function (arctan) will give you the angle from the origin to a point. For example: given (x,y), arctan(y/x) = the angle from the origin to the point (x,y).

If you search CD for something like “arctan” or “trig” or “trigonometry”, you should get a fair amount of results already covering implementation of trigonometric functions on the FRC controller.

I don’t have any experience using these, myself, but I’ve heard that a few teams have had success with using an arbitrary degree Taylor series approximation of trig functions.

Doesn’t arc tangent return an angle for a given slope? As in: -90 to 90.
I’m talking about 360 degrees. Like a Rectangular to Polar converter thing.

You then look at the signs of X and Y to see if what quadrant the angle is in.

:WHAP: Duh

``````
unsigned int GetAngle(signed int X, signed int Y)
{
signed int Angle = arctan(Y/abs(X));
unsigned char OtherSide = IsNegetive(X);
unsigned int Value = 0;

if (OtherSide)
Value = 180 - Angle + 360;
else
Value = Angle + 360;

Value %= 360;
return Value;
}
``````

Unfortunately, you have to use a float for Angle because angles between -45 and 45 are as important as the others. I suppose I could flip X and Y around for that.

[Changed that tan to arctan]

I don’t know much about programming, but I screwed around with winamp AVS for a while, and on that there was a function known as “atan2(x,y)” which was the same as atan but it automatically corrected for quadrants… isn’t there anything simular in C? I mean, C is much more powerful than that AVS junk from what I understand, it should at least have the same capabilities.

You don’t need a float to represent (Y/X). Just try ((Y*100)/X) and scale your arctangent function to reflect this. This works well if you are using a lookup table. However, I would use a float if I was using a McLaurin series. Otherwise you are likely to overload a long the when you start getting into the higher powers (100^5 is way too big). Also you might want to check to make sure X is not zero before dividing by it.

IFI doen not provide us with libraries of functions like this, but C certainly has them. The standard atan function requires more prcessing and floating point math than most FIRST programmers want to put in thier RC

This is the formula that LeoM posted a while back to get the angle from rectangular coordinates. Worked extremely well back in 2002

Temp1 was the positive distance in X direction
Temp2 was the positive distance in Y direction
temp3 = (temp2127) / SQR((temp1temp1)+(temp2*temp2)) 'Make values on a 127 unit circle

ss_degrees = ((1-(temp3/118))(((temp3+181)temp3+744)/769))+((temp3/118)(((((3temp3-707)*temp3+42611)/20))+((temp3-115)/3)))

[size=1]ss_degrees = (ss_degrees*180)/128 'Convert to Degrees
[/size]

Ay. The problem is I don’t get what it’s doing.

My current code is like this:

``````
#define abs(Num) ( ((Num) < 0) ? (0 - (Num)) : (Num) )
#define IsNegetive(Num) ( ((Num) < 0) ? 255 : 0 )
#define IsPositive(Num) ( ((Num) > 0) ? 255 : 0 )

/* 360 degrees = 256
0 =   0 (0x00)
45 =  32 (0x20)
90 =  64 (0x40)
135 =  96 (0x60)
180 = 128 (0x80)
225 = 160 (0xA0)
270 = 192 (0xC0)
315 = 224 (0xE0) */

unsigned char GetAngle(signed char X, signed char Y)
{
signed int  Angle = 0;
unsigned char OtherSide = (X < 0) ? 255 : 0;
unsigned char Value = 0;
unsigned char Simple = (X == 0) | (Y == 0) | (X == Y);

//Perform Simple tests
if (Simple)
{
if (X == 0)
if (Y < 0)
Angle = 192; // Down
else if (Y > 0)
Angle = 64; //Up
else // Y == 0
Angle = 0; //None, default

if (Y == 0)
if (X < 0)
Angle = 128; // Left
else if (X > 0)
Angle = 0; //Right
else // X == 0
Angle = 0; //None, default

if (X == Y)
{
switch( (IsNegetive(X) & 2) | (IsNegetive(Y) & 1) )
{
case 0: //++
Angle = 32;
break;
case 2: //-+
Angle = 96;
break;
case 3: //--
Angle = 160;
break;
case 1: //+-
Angle = 224;
break;
}
}
}

//Again for goto, no braces
if (Simple) goto Done; //I hope that's right

if (X > Y) //Preserve accuracy
Angle = 64 - atan(abs(X)/Y);
else
Angle = atan(abs(X)/Y);

Angle = OtherSide ? (127 - Angle) : Angle;
Value = (unsigned char)((Angle + 255) % 255);

Done:
return Value;
}
``````

Jamie,

The best advice I’ve ever heard about optimization is “Don’t – until you need to.” You generally want to get your code working, and only then worry about optimizing – if necessary.

I was refeing to post 1:

[quote=“Astronouth7303”]

: Why would I care? I was refeing to post 1:[/quote]

OK. And now I’m confused. I don’t understand “And One Of Astronouth’s Rare Contensts”, so I don’t know how to answer “Why would I care?” (I originally thought that line was part of your signature, so I just ignored it.)

It was a joke. The prize: Absolutely nothing!

Basically, I’m asking if anyone knows why you would want it on the controller.

Here is my Arc Tangent code:

``````#define ATAN_COUNT 64
char rom ATAN_ARRAY = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 14, 15, 16,
17, 18, 19, 21, 22, 23, 25, 26, 27, 29, 30, 32, 34,
36, 37, 39, 41, 43, 46, 48, 50, 53, 56, 59, 62, 66,
69, 74, 78, 83, 88, 95, 101, 109, 118, 128, 140, 154,
171, 192, 218, 253, 299, 367, 473, 663, 1106, 3319};

char atan (char Num)
{
char Index = 0;
char Value = 0;
char IsDone = 0;

LOOP:
if (ATAN_ARRAY[Index] > Num)
Value--;
if (ATAN_ARRAY[Index] >= Num)
IsDone = 255;
else if (Index >= ATAN_COUNT - 1) //was ATAN_COUNT + 1
{
IsDone = 255;
Value = 64;
}

if (IsDone)
goto LOOP_DONE;
else
{ Value++; Index++; }
goto LOOP;
LOOP_DONE:

return Value;
}
``````

The lookup table is compressed. If I were to use a regular variety than it would be 3320 elements long! Instead, I put the location of the element that the value changes. That’s why I loop. I think this will work, but I’m not sure.

that chould be ATAN_COUNT - 1 not ATAN_COUNT + 1 at line 19[/edit]

hmmm you can also use vectors, and find the angle with the formula (u * v)/(|u||v|) (the dot product of u and v divided by the product of the magnitudes). This is useful if you are already using vectors, which are probably better suited for motion and such anyways.

The result of that equation gives you the cos of the angle between the vectors. That is cos(x). You would still need to use an inverse trig function to find the angle (x).

I’m going to have to go ahead and give you -15 points for that one. o_O

`` -Daniel``