1  /********************************************************************
  2  * Description: maxkins.c
  3  *   Kinematics for Chris Radek's tabletop 5 axis mill named 'max'.
  4  *   This mill has a tilting head (B axis) and horizontal rotary
  5  *   mounted to the table (C axis).
  6  *
  7  * Author: Chris Radek
  8  * License: GPL Version 2
  9  *    
 10  * Copyright (c) 2007 Chris Radek
 11  ********************************************************************/
 12  
 13  #include "kinematics.h"		/* these decls */
 14  #include "posemath.h"
 15  #include "hal.h"
 16  #include "rtapi.h"
 17  #include "rtapi_math.h"
 18  
 19  #define d2r(d) ((d)*PM_PI/180.0)
 20  #define r2d(r) ((r)*180.0/PM_PI)
 21  
 22  #ifndef hypot
 23  #define hypot(a,b) (sqrt((a)*(a)+(b)*(b)))
 24  #endif
 25  
 26  struct haldata {
 27      hal_float_t *pivot_length;
 28  } *haldata;
 29  
 30  int kinematicsForward(const double *joints,
 31  		      EmcPose * pos,
 32  		      const KINEMATICS_FORWARD_FLAGS * fflags,
 33  		      KINEMATICS_INVERSE_FLAGS * iflags)
 34  {
 35      // B correction
 36      double zb = (*(haldata->pivot_length) + joints[8]) * cos(d2r(joints[4]));
 37      double xb = (*(haldata->pivot_length) + joints[8]) * sin(d2r(joints[4]));
 38          
 39      // C correction
 40      double xyr = hypot(joints[0], joints[1]);
 41      double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]);
 42  
 43      // U correction
 44      double zv = joints[6] * sin(d2r(joints[4]));
 45      double xv = joints[6] * cos(d2r(joints[4]));
 46  
 47      // V correction is always in joint 1 only
 48  
 49      pos->tran.x = xyr * cos(xytheta) + xb - xv;
 50      pos->tran.y = xyr * sin(xytheta) - joints[7];
 51      pos->tran.z = joints[2] - zb + zv + *(haldata->pivot_length);
 52  
 53      pos->a = joints[3];
 54      pos->b = joints[4];
 55      pos->c = joints[5];
 56      pos->u = joints[6];
 57      pos->v = joints[7];
 58      pos->w = joints[8];
 59  
 60      return 0;
 61  }
 62  
 63  int kinematicsInverse(const EmcPose * pos,
 64  		      double *joints,
 65  		      const KINEMATICS_INVERSE_FLAGS * iflags,
 66  		      KINEMATICS_FORWARD_FLAGS * fflags)
 67  {
 68      // B correction
 69      double zb = (*(haldata->pivot_length) + pos->w) * cos(d2r(pos->b));
 70      double xb = (*(haldata->pivot_length) + pos->w) * sin(d2r(pos->b));
 71          
 72      // C correction
 73      double xyr = hypot(pos->tran.x, pos->tran.y);
 74      double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c);
 75  
 76      // U correction
 77      double zv = pos->u * sin(d2r(pos->b));
 78      double xv = pos->u * cos(d2r(pos->b));
 79  
 80      // V correction is always in joint 1 only
 81  
 82      joints[0] = xyr * cos(xytheta) - xb + xv;
 83      joints[1] = xyr * sin(xytheta) + pos->v;
 84      joints[2] = pos->tran.z + zb + zv - *(haldata->pivot_length);
 85  
 86      joints[3] = pos->a;
 87      joints[4] = pos->b;
 88      joints[5] = pos->c;
 89      joints[6] = pos->u;
 90      joints[7] = pos->v;
 91      joints[8] = pos->w;
 92  
 93      return 0;
 94  }
 95  
 96  KINEMATICS_TYPE kinematicsType()
 97  {
 98      return KINEMATICS_BOTH;
 99  }
100  
101  #include "rtapi.h"		/* RTAPI realtime OS API */
102  #include "rtapi_app.h"		/* RTAPI realtime module decls */
103  
104  EXPORT_SYMBOL(kinematicsType);
105  EXPORT_SYMBOL(kinematicsInverse);
106  EXPORT_SYMBOL(kinematicsForward);
107  MODULE_LICENSE("GPL");
108  
109  int comp_id;
110  int rtapi_app_main(void) {
111      int result;
112      comp_id = hal_init("maxkins");
113      if(comp_id < 0) return comp_id;
114  
115      haldata = hal_malloc(sizeof(struct haldata));
116  
117      result = hal_pin_float_new("maxkins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
118      if(result < 0) goto error;
119  
120      *(haldata->pivot_length) = 0.666;
121  
122      hal_ready(comp_id);
123      return 0;
124  
125  error:
126      hal_exit(comp_id);
127      return result;
128  }
129  
130  void rtapi_app_exit(void) { hal_exit(comp_id); }