getting angle and distance to a set of coordinates

I'm creating a robot using the Atmel atmega644pa micro-controller and programming it in C. I am trying to create a function where given a pair of coordinates, it will determine the distance to that coordinate (from the robots current position) and the angle that it needs to turn to (relative to itself).

I'm having a problem with my angle calculations - no matter what coordinates I give it, it always calculates an angle greater than zero and sends the robot into a spin. Also, if I explicitly set my_bearing() to return 0.0, it all works correctly. Am I tackling getting this angle the correct way?

I haven't included the turn() or move() methods because they only set the pins to either turn or move. The functions I am having trouble with only set the angle and distance - a set of interrupt service routines do the rest.

#define MY_START_X          7.5
#define MY_START_Y          1.5
#define MY_START_BEARING    -90

float my_x=MY_START_X, my_y=MY_START_Y;
float go_x, go_y;
volatile float bearing=MY_START_BEARING

float my_bearing(void)
{
    float goto_x = go_x - my_x;                         // Adjust x to be relative to Me. I AM ORIGIN
    float goto_y = go_y - my_y;                         // Adjust y to be relative to Me. I AM ORIGIN
    float final = atan2f(goto_y,goto_x)*(180/3.14)-bearing;
    return final;   // Calculate my bearing
}

float my_distance(void)
{
    return sqrt((go_x-my_x)*(go_x-my_x)+(go_y-my_y)*(go_y-my_y));
}
void waypoint(float x, float y)
{

    go_x = x;                                   // Set my x to goto
    go_y = y;                                   // Set my y to goto

    bearing = my_bearing();                     // Get my bearing
    if(abs(bearing) > 1.0)
    {
        turn(bearing);                              // Turn Accordingly
        leave = 0;
        while(leave开发者_高级运维 == 0){};        // wait for ISR to run
    }
    move(FORWARD, my_distance());   // Go to my new x,y
    leave = 0;
    while(leave == 0){};               // wait for ISR to run
}

You're not getting the angle the right way. For starters, you're not accounting for corner cases (what about (0,2)? Means you end up doing 2/0). You're also not accounting for the quadrant. -2,-2 is a different bearing than 2,2 but in both cases x/y = 1.

Use atan2. It accounts for the quadrant.

If you don't have atan2, then you need to work out the quadrant yourself, and then using the quadrant work out the offset.