⼆⾃由度舵机云台球⾯⾓度、坐标转换笔记α 为向量与 XOY 平⾯夹⾓，β 为向量在 XOY 平⾯的投影与 OX 轴夹⾓。
所以可得如下公式：
x=r∗cosα∗cosβ
y=r∗cosα∗sinβ
z=r∗sinα
范围：−π
2<=α<=
π
2，0<=β<=2π
所以按照上述公式计算 (x,y,z) 坐标时，只要求出 α ⾓和 β ⾓即可。
在写程序前，需要⼀个能够计算 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;
}
float up
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_l
eft_pwm - dw_servo_right_pwm);    dinate_x = r * Servo_Cos(_angle_alpha) * Servo_Cos(_angle_beta);
dinate_y = r * Servo_Cos(_angle_alpha) * Servo_Sin(_angle_beta);
dinate_z = r * Servo_Sin(_angle_alpha);
}
Processing math: 100%