// Geometra robot

// (Ver fotos arriba para la explicacin)

e float const = 115,0; // Efector final
const float f = 457,3; // Base de
flotador re const = 232,0;
const float rf = 112,0;
// Constantes trigonomtricas
const float sqrt3 = sqrt (3.0);
const float pi = 3,141592653; // PI
const sin120 float = sqrt3 / 2.0;
float const cos120 = -0,5;
float const tan60 = sqrt3;
const sin30 float = 0,5;
float const tan30 = 1 / sqrt3;
// Cinemtica directa: (theta1, theta2, theta3) -> (x0, y0, z0)
// Estado devuelto: 0 = OK, -1 = posicin de no-existente
int delta_calcForward (theta1 flotador, flotar theta2, flotar theta3, flotar y
x0, y0 y flotar, flotar y z0) {
flotador t = (fe) * tan30 / 2;
pad float = pi / (float) 180,0;
theta1 * = pad;
theta2 * = pad;
theta3 * = pad;
y1 float = - (t + rf * cos (theta1));
flotar z1 = -rf * sin (theta1);
y2 = float (t + rf * cos (theta2)) * sin30;
flotar x2 = y2 * tan60;
flotar z2 = -rf * sin (theta2);
y3 float = (t + rf * cos (theta3)) * sin30;
flotador x3 = -Y3 * tan60;
flotar z3 = -rf * sin (theta3);
flotador DNM = (y2-y1) * x3- (y3-y1) * x2;
flotar w1 = y1 + z1 * Y1 * Z1;
flotar w2 = x2 * x2 + y2 + z2 * y2 * z2;
flotar w3 = x3 * x3 + y3 * y3 + z3 z3 *;
// X = (a1 * z + b1) / DNM
a1 float = (z2-z1) * (y3-y1) - (z3-z1) * (y2-y1);
b1 float = - ((W2-W1) * (y3-y1) - (w3-w1) * (y2-y1)) / 2.0;
// Y = (a2 + b2 * z) / DNM;
flotador a2 = - (z2-z1) * x3 + (z1 z3) * x2;
b2 float = ((W2-W1) * x3 - (w3-w1) * x2) /2.0;
// A * z ^ 2 + b * z + c = 0
flotar a = a1 * A1 + A2 * a2 + DNM * DNM;
float b = 2 * (a1 * b1 + a2 * (b2-y1 * DNM) - z1 * DNM * DNM);
flotador c = (b2-y1 * DNM) * (b2-y1 * DNM) + b1 * b1 + DNM * DNM * (* z1 z1
- re * re);
// Discriminante
flotador d = b * b - (float) 4.0 * a * c;

si (d <0) return -1; // Punto inexistente

z0 = - (float) 0,5 * (b + sqrt (d)) / a;
x0 = (a1 * z0 + b1) / DNM;
y0 = (a2 + b2 * z0) / DNM;
return 0;
}
// Cinemtica inversa
// Funciones de ayuda, calcula theta1 ngulo (para YZ-panel)
int delta_calcAngleYZ (x0 flotador, y0 flotador, z0 flotador, flotador y theta)
{
flotar y1 = -0.5 * 0,57735 * f; // F / 2 * tg 30
y0 - = 0.5 * 0,57735 * e; // Centro de cambio hacia el borde
// Z = a + b * y
flotar a = (x0 x0 * + * y0 y0 + z0 * z0 + rf rf * - * re re - y1 * y1) / (2
* z0);
float b = (y1-y0) / z0;
// Discriminante
flotar d = - (a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf);
si (d <0) return -1; // Punto inexistente
flotar yj = (y1 - a * b - sqrt (d)) / (b * b + 1); // Elegir punto exterior
flotar zj = a + b * yj;
theta = 180,0 * atan (-zj / (y1 - yj)) / pi + ((yj> y1) 180.0: 0,0);
return 0;
}
// Cinemtica inversa: (x0, y0, z0) -> (theta1, theta2, theta3)
// Estado devuelto: 0 = OK, -1 = posicin de no-existente
int delta_calcInverse (x0 flotador, y0 flotador, flotar z0, flotador y theta1,
flotar y theta2, flotar y theta3) {
theta1 = theta2 = theta3 = 0;
int estado = delta_calcAngleYZ (x0, y0, z0, theta1);
si (== estado 0) estado = delta_calcAngleYZ (x0 * cos120 + y0 * sin120, y0
* cos120-x0 * sin120, z0, theta2); // Girar a 120 grados coords
si (estado == 0) estado = delta_calcAngleYZ (x0 * cos120 - y0 * sin120, y0
* cos120 + x0 * sin120, z0, theta3); // Gire coords a -120 grados
devolver el estado;
}
robot delta
// Geometra robot
// (Ver fotos arriba para la explicacin)
const float e = 115,0;
// efector final
const float f = 457,3;
// base de
const float re = 232,0;
const float rf = 112,0;
// Constantes trigonomtricas
const float sqrt3 = sqrt (3.0);
const float pi = 3,141592653;
// PI
const float sin120 = sqrt3 / 2.0;
const float cos120 = - 0,5;
const float tan60 = sqrt3;
const float sin30 = 0,5;
const float tan30 = 1 / sqrt3;
// Cinemtica directa: (theta1, theta2, theta3) - & gt; (x0, y0, z0)
// Estado devuelto: 0 = OK, -1 = posicin de no-existente
int delta_calcForward (float theta1, flotar theta2, flotador theta3, flotar y
amp; x0, flotar y amp; y0, flotan y amp; z0) {

flotador t = (f - e) * tan30 / 2;
flotador pad = pi / (float) 180,0;
theta1 * = pad;
theta2 * = pad;
theta3 * = pad;
flotador y1 = - (t + rf * cos (theta1));
flotar z1 = - rf * pecado (theta1);
flotador y2 = (t + rf * cos (theta2)) * sin30;
flotar x2 = y2 * tan60;
flotar z2 = - rf * pecado (theta2);
flotador y3 = (t + rf * cos (theta3)) * sin30;
flotar x3 = - y3 * tan60;
flotar z3 = - rf * pecado (theta3);
flotador DNM = (y2 - y1) * x3 - (y3 - y1) * x2;
flotar w1 = y1 * y1 + z1 * Z1;
flotar w2 = x2 * x2 + y2 * y2 + z2 * z2;
flotar w3 = x3 * x3 + y3 * y3 + z3 * z3;
// X = (a1 * z + b1) / DNM
flotador a1 = (z2 - z1) * (y3 - y1) - (z3 - z1) * (y2 - y1);
flotador b1 = - ((w2 - w1) * (y3 - y1) - (w3 - w1) * (y2 - y1)) / 2.0;
// Y = (a2 + b2 * z) / DNM;
flotar a2 = - (z2 - z1) * x3 + (z3 - z1) * x2;
flotador b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0;
// A * z ^ 2 + b *
flotar a = a1 * a1
float b = 2 * (a1
flotador c = (b2 * z1 - re * re);

z + c = 0
+ a2 * a2 + DNM * DNM;
* b1 + a2 * (b2 - y1 * DNM) - z1 * DNM * DNM);
y1 * DNM) * (b2 - y1 * DNM) + b1 * b1 + DNM * DNM * (z1

// Discriminante
flotador d = b * b - (float) 4.0 * un * c;
si (d y lt; 0) retorno - 1; // punto inexistente
z0 = - (float) 0,5 * (b + sqrt (d)) / a;
x0 = (a1 * z0 + b1) / DNM;
y0 = (a2 * z0 + b2) / DNM;
volver 0;
}
// Cinemtica inversa
// Funciones de ayuda, calcula theta1 ngulo (para YZ-panel)
int delta_calcAngleYZ (float x0, flotador y0, flotar z0, flotador y theta;) {
flotador y1 = - 0,5 * 0,57735 * f; // f / 2 * tg 30
y0 - = 0.5 * 0,57735
* e;
centro // desplazamiento hasta el bord
e
// Z = a + b * y
flotar un = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (
2 * z0);
float b = (y1 - y0) / z0;
// Discriminante
flotar d = - (a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf);

si (d y lt; 0) retorno - 1; // punto inexistente

flotar yj = (y1 - un * b - sqrt (d)) / (b * b + 1); // eleccin de punto ex
terior
flotar zj = un + b * yj;
theta = 180,0 * atan (- zj / (y1 - yj)) / pi + ((yj y gt; y1)? 180.0: 0,0)
;
volver 0;
}
// Cinemtica inversa: (x0, y0, z0) - & gt; (theta1, theta2, theta3)
// Estado devuelto: 0 = OK, -1 = posicin de no-existente
int delta_calcInverse (float x0, flotador y0, flotar z0, flotador y amp; thet
a1, flotar y amp; theta2, flotar y amp; theta3) {
theta1 = theta2 = theta3 = 0;
int status = delta_calcAngleYZ (x0, y0, z0, theta1);
si (estado == 0) Estado = delta_calcAngleYZ (x0 * cos120 + y0 * sin120, y
0 * cos120 - x0 * sin120, z0, theta2); // rote coords a +120 grados
si (estado == 0) Estado = delta_calcAngleYZ (x0 * cos120 - y0 * sin120, y
0 * cos120 + x0 * sin120, z0, theta3); // rote coords a -120 grados
devolver el estado;
}