获取到一组坐标的角度和距离

Question

我正在使用 Atmel atmega644pa 微控制器创建一个机器人并用 C 对其进行编程。我正在尝试创建一个函数，其中给定一对坐标，它将确定到该坐标的距离（从机器人当前位置) 以及它需要转向的角度（相对于自身）。

我的角度计算有问题 - 无论我给它什么坐标，它总是计算一个大于零的角度并使机器人旋转。此外，如果我将 my_bearing() 显式设置为返回 0.0，则一切正常。我是否以正确的方式处理这个角度？

我没有包含 turn() 或 move() 方法，因为它们仅将引脚设置为转动或移动。我遇到问题的功能只设置角度和距离 - 一组中断服务例程完成其余的工作。

#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
}

Answer 1

您没有以正确的方式获得角度。 对于初学者来说，你没有考虑极端情况（（0,2）呢？意味着你最终会做 2/0）。 你也没有考虑象限。 -2,-2 与 2,2 的方位角不同，但在这两种情况下 x/y = 1。

使用 atan2。它占象限。

如果你没有atan2，那么你需要自己算出象限，然后用象限算出偏移量。