二自由度舵机云台球面角度、坐标转换笔记

(alpha) 为向量与 XOY 平面夹角，(eta) 为向量在 XOY 平面的投影与 OX 轴夹角。

所以可得如下公式：

• (x = r * cosalpha * coseta)
• (y = r * cosalpha * sineta)
• (z = r * sinalpha)

范围：(dfrac{-pi}{2}<=alpha<=dfrac{pi}{2})，(0<=eta<=2pi)

所以按照上述公式计算 (left(x,y,z ight)) 坐标时，只要求出 (alpha) 角和 (eta) 角即可。

在写程序前，需要一个能够计算 cos 和 sin 的函数如下：

const double kServoPi = 3.14159265; // π

// sin(x) 函数
double Servo_Sin(double rad) {
    int8 _flag = 1;
    double sine;

    if (rad >= kServoPi) {
        rad -= kServoPi;
        _flag = -1;
    }

    if (rad < 0)
        sine = rad * (1.27323954f + 0.405284735f * rad);
    else
        sine = rad * (1.27323954f - 0.405284735f * rad);
    if (sine < 0)
        sine = sine * (-0.225f * (sine + 1) + 1);
    else
        sine = sine * (0.225f * (sine - 1) + 1);

    return sine * _flag;
}

// cos(x) 函数
float Servo_Cos(double rad) {
    int8 _flag = 1;
    rad += kServoPi / 2.0;

    if (rad >= kServoPi) {
        _flag = -1;
        rad -= kServoPi;
    }

    return Servo_Sin(rad) * _flag;
}

准备完成后，开始计算舵机角度：

struct PanTilt {
    float coordinate_x;  // 云台 x 轴坐标
    float coordinate_y;  // 云台 y 轴坐标
    float coordinate_z;  // 云台 z 轴坐标
};

struct PanTilt pan_tilt = {0, 0, 0};

// 舵机角度计算
void Servo_Coordinates_Calculate(uint16 _pwm1, uint16 _pwm2) {
    const float up_servo_high_pwm = 1105, up_servo_mid_pwm = 672.5,
                up_servo_low_pwm = 240;  // 上舵机
    const float dw_servo_left_pwm = 1105, dw_servo_right_pwm = 234;  // 下舵机
    const float r = 51;  // 半径为 51mm

    static float _angle_alpha = 0, _angle_beta = 0;  // α 和 β 角

    _angle_alpha = kServoPi * ((float)(_pwm1)-up_servo_mid_pwm) / (up_servo_high_pwm - up_servo_low_pwm);
    _angle_beta = kServoPi * ((float)(_pwm2)-dw_servo_right_pwm) / (dw_servo_left_pwm - dw_servo_right_pwm);

    pan_tilt.coordinate_x = r * Servo_Cos(_angle_alpha) * Servo_Cos(_angle_beta);
    pan_tilt.coordinate_y = r * Servo_Cos(_angle_alpha) * Servo_Sin(_angle_beta);
    pan_tilt.coordinate_z = r * Servo_Sin(_angle_alpha);
}