Rev 1215 |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
#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;
}