// 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 (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 * 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 * 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 borde // 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 exterior 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; theta1, 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, y0 * cos120 - x0 * sin120, z0, theta2); // rote coords a +120 grados si (estado == 0) Estado = delta_calcAngleYZ (x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z0, theta3); // rote coords a -120 grados devolver el estado; }