0,0 → 1,85 |
#include <kernel/kern.h> |
#include <math.h> |
|
#include "servo.h" |
|
#define LEG_A 10.0 |
#define LEG_B 10.0 |
#define LEG_C 2.0 |
#define LEG_D 3.0 |
#define LEG_CD_2IPO 2*3.603 // 2*sqrt(LEG_C^2+LEG_D^2) |
#define LEG_CD_ANG 33.690 // arctg(LEG_C/LEG_D) in gradi |
|
const double c0 = LEG_C * LEG_C; |
const double c1 = LEG_B * LEG_B; |
const double c2 = LEG_B * LEG_B - LEG_A * LEG_A; |
const double todeg = 180.0 / PI; |
|
int set_leg_position(double px, double py, double pz) |
{ |
|
double px2 = px * px; |
double py2 = py * py; |
double pz2 = pz * pz; |
|
double pxz2 = px2 + pz2; |
|
int alfa,beta,gamma; |
double alfa1,beta1,alfa2,beta2,gamma1; |
double m,dsqrt; |
|
double delta_xz = pxz2 - c0; |
double s,k,k2,y1,delta_xy; |
|
if (delta_xz < 0.0) return -1; |
|
if (pz >= LEG_C) { |
gamma1 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg; |
} else { |
gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg; |
} |
|
if (gamma1 < -90.0 || gamma1 > 90.0) return -1; |
|
m = pxz2 - LEG_CD_2IPO * (sin(gamma+LEG_CD_ANG)+cos(gamma+LEG_CD_ANG)); |
|
s = m + py2; |
k = c2 + s; |
k2 = k * k; |
|
delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1); |
|
if (delta_xy >= 0.0) { |
|
dsqrt = sqrt(delta_xy); |
y1 = (py * k + dsqrt) / (2.0 * s); |
beta1 = asin(y1/LEG_B) * todeg; |
alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1; |
y1 = (py * k - dsqrt) / (2.0 * s); |
beta2 = asin(y1/LEG_B) * todeg; |
alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2; |
|
if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) { |
alfa = (int)(alfa1 * 3600.0); |
beta = (int)(beta1 * 3600.0); |
gamma = (int)(gamma1 * 3600.0); |
cprintf("Alfa Sec = %d Beta Sec = %d Gamma Sec = %d\n",alfa,beta,gamma); |
return 0; |
} else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) { |
alfa = (int)(alfa2 * 3600.0); |
beta = (int)(beta2 * 3600.0); |
gamma = (int)(gamma1 * 3600.0); |
cprintf("Alfa Sec = %d Beta Sec = %d Gamma Sec = %d\n",alfa,beta,gamma); |
return 0; |
} else { |
cprintf("No possible !\n"); |
return 1; |
} |
|
} else |
return 1; |
|
return 0; |
|
} |
|