cnc-club.ru

Код: Выделить всё

   1 /********************************************************************
   2 * Description: trivkins.c
   3 *   Trivial kinematics for 3 axis Cartesian machine
   4 *
   5 *   Derived from a work by Fred Proctor & Will Shackleford
   6 *
   7 * Author:
   8 * License: GPL Version 2
   9 * System: Linux
  10 *    
  11 * Copyright (c) 2004 All rights reserved.
  12 *
  13 * Last change:
  14 ********************************************************************/
  15 
  16 #include "kinematics.h"         /* these decls */
  17 
  18 
  19 int kinematicsForward(const double *joints,
  20                       EmcPose * pos,
  21                       const KINEMATICS_FORWARD_FLAGS * fflags,
  22                       KINEMATICS_INVERSE_FLAGS * iflags)
  23 {
  24     pos->tran.x = joints[0];
  25     pos->tran.y = joints[1];
  26     pos->tran.z = joints[2];
  27     pos->a = joints[3];
  28     pos->b = joints[4];
  29     pos->c = joints[5];
  30     pos->u = joints[6];
  31     pos->v = joints[7];
  32     pos->w = joints[8];
  33 
  34     return 0;
  35 }
  36 
  37 int kinematicsInverse(const EmcPose * pos,
  38                       double *joints,
  39                       const KINEMATICS_INVERSE_FLAGS * iflags,
  40                       KINEMATICS_FORWARD_FLAGS * fflags)
  41 {
  42     joints[0] = pos->tran.x;
  43     joints[1] = pos->tran.y;
  44     joints[2] = pos->tran.z;
  45     joints[3] = pos->a;
  46     joints[4] = pos->b;
  47     joints[5] = pos->c;
  48     joints[6] = pos->u;
  49     joints[7] = pos->v;
  50     joints[8] = pos->w;
  51 
  52     return 0;
  53 }
  54 
  55 /* implemented for these kinematics as giving joints preference */
  56 int kinematicsHome(EmcPose * world,
  57                    double *joint,
  58                    KINEMATICS_FORWARD_FLAGS * fflags,
  59                    KINEMATICS_INVERSE_FLAGS * iflags)
  60 {
  61     *fflags = 0;
  62     *iflags = 0;
  63 
  64     return kinematicsForward(joint, world, fflags, iflags);
  65 }
  66 
  67 KINEMATICS_TYPE kinematicsType()
  68 {
  69     return KINEMATICS_IDENTITY;
  70 }
  71 
  72 #include "rtapi.h"              /* RTAPI realtime OS API */
  73 #include "rtapi_app.h"          /* RTAPI realtime module decls */
  74 #include "hal.h"
  75 
  76 EXPORT_SYMBOL(kinematicsType);
  77 EXPORT_SYMBOL(kinematicsForward);
  78 EXPORT_SYMBOL(kinematicsInverse);
  79 MODULE_LICENSE("GPL");
  80 
  81 int comp_id;
  82 int rtapi_app_main(void) {
  83     comp_id = hal_init("trivkins");
  84     if(comp_id > 0) {
  85         hal_ready(comp_id);
  86         return 0;
  87     }
  88     return comp_id;
  89 }
  90 
  91 void rtapi_app_exit(void) { hal_exit(comp_id); }

Код: Выделить всё

   1 /********************************************************************
   2 * Description: 5axiskins.c
   3 *   Trivial kinematics for 3 axis Cartesian machine
   4 *
   5 *   Derived from a work by Fred Proctor & Will Shackleford
   6 *
   7 * Author:
   8 * License: GPL Version 2
   9 * System: Linux
  10 *    
  11 * Copyright (c) 2007 Chris Radek
  12 *
  13 * Last change:
  14 ********************************************************************/
  15 
  16 #include "kinematics.h"         /* these decls */
  17 #include "posemath.h"
  18 #include "hal.h"
  19 #include "rtapi_math.h"
  20 
  21 #define d2r(d) ((d)*PM_PI/180.0)
  22 #define r2d(r) ((r)*180.0/PM_PI)
  23 
  24 struct haldata {
  25     hal_float_t *tool_length;
  26     hal_float_t *pivot_length;
  27 } *haldata;
  28 
  29 static PmCartesian s2r(double r, double t, double p) {
  30     PmCartesian c;
  31     t = d2r(t), p = d2r(p);
  32 
  33     c.x = r * sin(p) * cos(t);
  34     c.y = r * sin(p) * sin(t);
  35     c.z = r * cos(p);
  36 
  37     return c;
  38 }
  39 
  40 int kinematicsForward(const double *joints,
  41                       EmcPose * pos,
  42                       const KINEMATICS_FORWARD_FLAGS * fflags,
  43                       KINEMATICS_INVERSE_FLAGS * iflags)
  44 {
  45     PmCartesian r = s2r(*(haldata->pivot_length) + joints[8], joints[5], 180.0 - joints[4]);
  46 
  47     pos->tran.x = joints[0] + r.x;
  48     pos->tran.y = joints[1] + r.y;
  49     pos->tran.z = joints[2] + *(haldata->pivot_length) + r.z;
  50     pos->a = joints[3];
  51     pos->b = joints[4];
  52     pos->c = joints[5];
  53     pos->u = joints[6];
  54     pos->v = joints[7];
  55     pos->w = joints[8];
  56 
  57     return 0;
  58 }
  59 
  60 int kinematicsInverse(const EmcPose * pos,
  61                       double *joints,
  62                       const KINEMATICS_INVERSE_FLAGS * iflags,
  63                       KINEMATICS_FORWARD_FLAGS * fflags)
  64 {
  65 
  66     PmCartesian r = s2r(*(haldata->pivot_length) + pos->w, pos->c, 180.0 - pos->b);
  67 
  68     joints[0] = pos->tran.x - r.x;
  69     joints[1] = pos->tran.y - r.y;
  70     joints[2] = pos->tran.z - *(haldata->pivot_length) - r.z;
  71     joints[3] = pos->a;
  72     joints[4] = pos->b;
  73     joints[5] = pos->c;
  74     joints[6] = pos->u;
  75     joints[7] = pos->v;
  76     joints[8] = pos->w;
  77 
  78     return 0;
  79 }
  80 
  81 /* implemented for these kinematics as giving joints preference */
  82 int kinematicsHome(EmcPose * world,
  83                    double *joint,
  84                    KINEMATICS_FORWARD_FLAGS * fflags,
  85                    KINEMATICS_INVERSE_FLAGS * iflags)
  86 {
  87     *fflags = 0;
  88     *iflags = 0;
  89 
  90     return kinematicsForward(joint, world, fflags, iflags);
  91 }
  92 
  93 KINEMATICS_TYPE kinematicsType()
  94 {
  95     return KINEMATICS_BOTH;
  96 }
  97 
  98 #include "rtapi.h"              /* RTAPI realtime OS API */
  99 #include "rtapi_app.h"          /* RTAPI realtime module decls */
 100 #include "hal.h"
 101 
 102 EXPORT_SYMBOL(kinematicsType);
 103 EXPORT_SYMBOL(kinematicsForward);
 104 EXPORT_SYMBOL(kinematicsInverse);
 105 MODULE_LICENSE("GPL");
 106 
 107 int comp_id;
 108 int rtapi_app_main(void) {
 109     int result;
 110     comp_id = hal_init("5axiskins");
 111     if(comp_id < 0) return comp_id;
 112 
 113     haldata = hal_malloc(sizeof(struct haldata));
 114 
 115     result = hal_pin_float_new("5axiskins.tooloffset.z", HAL_IN, &(haldata->tool_length), comp_id);
 116     if(result < 0) goto error;
 117     result = hal_pin_float_new("5axiskins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
 118     if(result < 0) goto error;
 119 
 120     *(haldata->pivot_length) = 250.0;
 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); }