Manual

51
- mathematical model 2/2
* ---------------------------------------------------------------------------*/
void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile
float &gamma, volatile float x, volatile float y, volatile float z)
{
//calculate w-z degree
float v, w;
w = (x >= 0 ? 1 : -1)*(sqrt(pow(x, 2) + pow(y, 2)));
v = w - length_c;
alpha = atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2)
+ pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2)));
beta = acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) /
2 / length_a / length_b);
//calculate x-y-z degree
gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x);
//trans degree pi->180
alpha = alpha / pi * 180;
beta = beta / pi * 180;
gamma = gamma / pi * 180;
}
First build a 3D model for a certain leg. The coordinate direction should be consistent with
that on the calibration chart, as shown below:
Here we'll only analyze the first quadrant of the leg end: given the end position Point (x,y,z)
and segment a, b, c (the length of each segment of the leg), to calculate the rotational
angle of the servo α, β, γ. Within, π/2≤α≤π/2，0≤β≤π，-π/2≤γ≤π/2. In this way,
transform these into a basic mathematic model. The proof of the model:
w= 

 

V=w-c
With the law of cosines, cos a =







, the result of ∠2 can be calculated.
SunFounder