⼆⾃由度舵机云台球⾯⾓度、坐标转换笔记α 为向量与 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;
}
if (rad < 0)
sine = rad * (1.27323954f + 0.405284735f * rad);
el
sine = rad * (1.27323954f - 0.405284735f * rad);
if (sine < 0)
sine = sine * (-0.225f * (sine + 1) + 1);
el
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;
will过去式
}
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_rvo_high_pwm = 1105, up_rvo_mid_pwm = 672.5,
up_rvo_low_pwm = 240; // 上舵机
const float dw_rvo_left_pwm = 1105, dw_rvo_right_pwm = 234; // 下舵机 const float r = 51; // 半径为 51mm
static float _angle_alpha = 0, _angle_beta = 0; // α和β⾓
_angle_alpha = kServoPi * ((float)(_pwm1)-up_rvo_mid_pwm) / (up_rvo_high_pwm - up_rvo_low_pwm); _angle_beta = kServoPi * ((float)(_pwm2)-dw_rvo_right_pwm) / (dw_rvo
_left_pwm - dw_rvo_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%
