Kod: Zaznacz cały

/********************************************************************
* Description: deltakins.c
*   Kinematics for 3 axis Delta machine
*   Derived from a work of mzavatsky at Trossen Robotics
*                          at forums.trossenrobotics.com
* Author: Jozsef Papp / hg5bsd 
*    and: István Ábel at kvarc.extra.hu
* License: coding and fitting it into EMC2 from our part is GPL alike
*          but we won't place the code itself under GPL because we
*          don't know if this would hurt or not the delta robot 
*          inventors rights, if you are shure that it doesn't hurt,
*          then you are free to place it under GPL ( in this case place
*          your name in the comment lines, and keep original comments ) 
* System: realtime Linux, EMC2
* Copyright (c) 2010 All rights reserved.
* Last change:                   2010.07.14.14.28.
********************************************************************/
#include "kinematics.h"             /* these decls */
#include "rtapi_math.h" 


/* robot geometry  /Ref - mzavatsky: Delta robot kinematics/

e = side length of end effector triangle, middle arm - "re" 
f = side length of base triangle, middle drive joints - "rf" 
re = length of end effector arm
rf = length of drive arm   

sample: 
e = 115.0; 
f = 457.3; 
re = 232.0; 
rf = 112.0;  */





#ifdef RTAPI
#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "hal.h" 

struct haldata { 
    hal_float_t *e, *f, *re, *rf; 
} *haldata = 0; 

#define delta_e (*(haldata->e))    
#define delta_f (*(haldata->f))    
#define delta_re (*(haldata->re))  
#define delta_rf (*(haldata->rf)) 

#else
double delta_e, delta_f, delta_re, delta_rf; 
#endif

 /* trigonometric constants */
 const double sqrt3 = 1.7320508075688772935274463415059;   /* sqrt(3.0);*/
#ifndef PI
#define PI 3.14159265358979323846
#endif
 const double sin120 = 0.86602540378443864676372317075294; /* (sqrt3)/2;*/   
 const double cos120 = -0.5;        
 const double tan60 = 1.7320508075688772935274463415059;   /* sqrt3;*/
 const double sin30 = 0.5; 
 const double tan30 = 0.57735026918962576450914878050196;  /* 1/(sqrt3);*/

  
 /* forward kinematics: (joints[0], joints[1], joints[2]) -> (pos->tran.x, pos->tran.y, pos->tran.z) 
 // returned status: 0=OK, -1=non-existing position*/

int kinematicsForward( const double *joints,
		      EmcPose *pos,
		      const KINEMATICS_FORWARD_FLAGS *fflags,
		      KINEMATICS_INVERSE_FLAGS *iflags) 

 { 





     double t = (delta_f-delta_e)*tan30/2; 
     
     /* float dtr = pi/(float)180.0; TO_RAD */
 
     double theta1 = joints[0] * TO_RAD; 
     double theta2 = joints[1] * TO_RAD; 
     double theta3 = joints[2] * TO_RAD; 
 
     double y1 = -(t + delta_rf*cos(theta1)); 
     double z1 = -delta_rf*sin(theta1); 
 
     double y2 = (t + delta_rf*cos(theta2))*sin30; 
     double x2 = y2*tan60; 
     double z2 = -delta_rf*sin(theta2); 
 
     double y3 = (t + delta_rf*cos(theta3))*sin30; 
     double x3 = -y3*tan60; 
     double z3 = -delta_rf*sin(theta3); 
 
     double dnm = (y2-y1)*x3-(y3-y1)*x2; 
 
     double w1 = y1*y1 + z1*z1; 
     double w2 = x2*x2 + y2*y2 + z2*z2; 
     double w3 = x3*x3 + y3*y3 + z3*z3; 
     
     /* x = (a1*z + b1)/dnm*/
     double a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1); 
     double b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0; 
 
     /* y = (a2*z + b2)/dnm;*/
     double a2 = -(z2-z1)*x3+(z3-z1)*x2; 
     double b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0; 
 
     /* a*z^2 + b*z + c = 0*/
     double a = a1*a1 + a2*a2 + dnm*dnm; 
     double b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm); 
     double c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - delta_re*delta_re); 
  
     /* discriminant*/
     double d = b*b - (double)4.0*a*c; 
     if (d < 0) return -1; /* non-existing point*/
 
     pos->tran.z = -(double)0.5*(b+sqrt(d))/a; 
     pos->tran.x = (a1*pos->tran.z + b1)/dnm; 
     pos->tran.y = (a2*pos->tran.z + b2)/dnm; 

     return 0; 

 } 


 
 /* inverse kinematics
    helper functions, calculates angle theta1 (for YZ-pane)*/
 int delta_calcAngleYZ( double x0, double y0, double z0, double *theta ) 
  { 
     double y1 = -0.5 * 0.57735 * delta_f; // f/2 * tg 30
     y0 -= 0.5 * 0.57735    * delta_e;    // shift center to edge
     /* z = a + b*y*/
     double a = (x0*x0 + y0*y0 + z0*z0 +delta_rf*delta_rf - delta_re*delta_re - y1*y1)/(2*z0); 
     double b = (y1-y0)/z0; 
     /* discriminant*/
     double d = -(a+b*y1)*(a+b*y1)+delta_rf*(b*b*delta_rf+delta_rf); 
     if (d < 0) return -1; /* non-existing point*/
     double yj = (y1 - a*b - sqrt(d))/(b*b + 1); /* choosing outer point*/
     double zj = a + b*yj; 
     *theta = TO_DEG * atan2(-zj,(y1 - yj)) + ((yj>y1)?180.0:0.0); 
     return 0; 
 } 
 
 /* inverse kinematics: (pos->tran.x, pos->tran.y, pos->tran.z) -> (joints[0], joints[1], joints[2]) 
    returned status: 0=OK, -1=non-existing position*/
int kinematicsInverse(const EmcPose *pos,
		      double *joints,
		      const KINEMATICS_INVERSE_FLAGS *iflags,
		      KINEMATICS_FORWARD_FLAGS *fflags) 
  
 { 

     double x0 = pos->tran.x; 
     double y0 = pos->tran.y; 
     double z0 = pos->tran.z; 
     double theta1; 
     double theta2; 
     double theta3; 
     
     theta1 = theta2 = theta3 = 0; 
     int status = delta_calcAngleYZ(x0, y0, z0, &theta1); 
     if (status == 0) joints[0] = theta1; 
     if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, &theta2); /*rotate coords to +120 deg*/
     if (status == 0) joints[1] = theta2; 
     if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, &theta3); /*rotate coords to -120 deg*/
     if (status == 0) joints[2] = theta3; 
     
     return status; 
 } 



 /* implemented for these kinematics as giving joints preference */
int kinematicsHome(EmcPose * world,
		   double *joint,
		   KINEMATICS_FORWARD_FLAGS * fflags,
		   KINEMATICS_INVERSE_FLAGS * iflags) 
{ 
    *fflags = 0; 
    *iflags = 0; 

    return kinematicsForward(joint, world, fflags, iflags); 
} 

KINEMATICS_TYPE kinematicsType() 
{ 
    return KINEMATICS_BOTH; 
} 

#ifdef RTAPI
#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "hal.h" 

EXPORT_SYMBOL(kinematicsType); 
EXPORT_SYMBOL(kinematicsForward); 
EXPORT_SYMBOL(kinematicsInverse); 
MODULE_LICENSE("GPL"); 



int comp_id; 
int rtapi_app_main(void) { 
    int res = 0; 

    comp_id = hal_init("deltakins"); 
    if(comp_id < 0) return comp_id; 

    haldata = hal_malloc(sizeof(struct haldata)); 
    if(!haldata) goto error; 
	
    if((res = hal_pin_float_new("deltakins.e", HAL_IO, &(haldata->e), comp_id)) != HAL_SUCCESS) goto error; 
    if((res = hal_pin_float_new("deltakins.f", HAL_IO, &(haldata->f), comp_id)) != HAL_SUCCESS) goto error; 
    if((res = hal_pin_float_new("deltakins.re", HAL_IO, &(haldata->re), comp_id)) != HAL_SUCCESS) goto error; 
    if((res = hal_pin_float_new("deltakins.rf", HAL_IO, &(haldata->rf), comp_id)) != HAL_SUCCESS) goto error; 

    delta_e = delta_f = delta_re = delta_rf = 1.0; 
    
    hal_ready(comp_id); 
    return 0; 

error: 
    hal_exit(comp_id); 
    return res; 
} 

void rtapi_app_exit(void) { hal_exit(comp_id); } 
#endif