Advanced Search

// opmode constants

variable char modeSwap;
variable char homeState;
;;

#define positionModeStatus                                              16

#define WRITEMODE 0
#define WAITVELOMODE 1
#define INITIAL 2
#define WORKING 3
#define WAITINGFORENABLE 4
#define HOMING 5

#define HOMETRIGGER 0
#define HOMECONFIRM 1
#define HOMESEEKING 2

component  xyzcb_tdr_kins "Switchable kinematics for 5 axis machine with rotary table C and B and virtual Y Axis";

description
"""

This is a switchable kinematics module for a 5-axis milling configuration
using 3 cartesian linear joints (XYZ) and 2 rotary table joints (CB).

The module contains two kinematic models:

type0 (default) is a trivial XYZCB configuration with joints 0..4 mapped to
axes XYZCB respectively.

type1 is a XYZCB configuration with tool center point (TCP) compensation.

Additionally a virtual Y Axis was added for a Mazak Integrex 200Y
""";

pin out s32 dummy=0"one pin needed to satisfy halcompile requirement";

license "GPL";
author "Jochen Rohm";
;;

#include "rtapi_math.h"
#include "kinematics.h"

static struct haldata {

  // Declare hal pin pointers used for XYZCB_tdr kinematics:
  hal_float_t *tool_offset_z;
  hal_float_t *x_offset;
  hal_float_t *z_offset;
  hal_float_t *x_rot_point;
  hal_float_t *y_rot_point;
  hal_float_t *z_rot_point;
  hal_float_t *angleXX;

  //Declare hal pin pointers used for switchable kinematics
  hal_bit_t   *kinstype_is_0;
  hal_bit_t   *kinstype_is_1;
} *haldata;

static int XYZCB_tdr_setup(void) {
#define HAL_PREFIX "xyzcb_tdr_kins"
    int res=0;
    // inherit comp_id from rtapi_main()
    if (comp_id < 0) goto error;
    // set unready to allow creation of pins
    if (hal_set_unready(comp_id)) goto error;

    haldata = hal_malloc(sizeof(struct haldata));
    if (!haldata) goto error;

    // hal pins required for XYZCB_tdr kinematics:
    res += hal_pin_float_newf(HAL_IN ,&(haldata->tool_offset_z) ,comp_id,"%s.tool-offset-z" ,HAL_PREFIX);
    res += hal_pin_float_newf(HAL_IN ,&(haldata->x_offset) ,comp_id,"%s.x-offset" ,HAL_PREFIX);
    res += hal_pin_float_newf(HAL_IN ,&(haldata->z_offset) ,comp_id,"%s.z-offset" ,HAL_PREFIX);
    res += hal_pin_float_newf(HAL_IN ,&haldata->x_rot_point ,comp_id,"%s.x-rot-point" ,HAL_PREFIX);
    res += hal_pin_float_newf(HAL_IN ,&haldata->y_rot_point ,comp_id,"%s.y-rot-point" ,HAL_PREFIX);
    res += hal_pin_float_newf(HAL_IN ,&haldata->z_rot_point ,comp_id,"%s.z-rot-point" ,HAL_PREFIX);

    // hal pins required for switchable kinematics:
    res += hal_pin_bit_new("kinstype.is-0", HAL_OUT, &(haldata->kinstype_is_0), comp_id);
    res += hal_pin_bit_new("kinstype.is-1", HAL_OUT, &(haldata->kinstype_is_1), comp_id);

    // define default kinematics at startup for switchable kinematics
    *haldata->kinstype_is_0 = 1; //default at startup -> identity kinematics
    *haldata->kinstype_is_1 = 0; //-> XYZCB TCP

    if (res) goto error;
    hal_ready(comp_id);
    rtapi_print("*** %s setup ok\n",__FILE__);
    return 0;
error:
    rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res);
    return -1;
#undef HAL_PREFIX
}

EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsSwitchable);
EXPORT_SYMBOL(kinematicsSwitch);
EXPORT_SYMBOL(kinematicsInverse);
EXPORT_SYMBOL(kinematicsForward);

static hal_u32_t switchkins_type;

int kinematicsSwitchable() {return 1;}

int kinematicsSwitch(int new_switchkins_type)
{
    switchkins_type = new_switchkins_type;
    rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type);
    // create case structure for switchable kinematics
    switch (switchkins_type) {
        case 0: rtapi_print_msg(RTAPI_MSG_INFO,
                "kinematicsSwitch:TYPE0\n");
                *haldata->kinstype_is_0 = 1;
                *haldata->kinstype_is_1 = 0;
                break;
        case 1: rtapi_print_msg(RTAPI_MSG_INFO,
                "kinematicsSwitch:TYPE1\n");
                *haldata->kinstype_is_0 = 0;
                *haldata->kinstype_is_1 = 1;
                break;
       default: rtapi_print_msg(RTAPI_MSG_ERR,
                "kinematicsSwitch:BAD VALUE <%d>\n",
                switchkins_type);
                *haldata->kinstype_is_1 = 0;
                *haldata->kinstype_is_0 = 0;
                return -1; // FAIL
    }
    return 0; // ok
}

KINEMATICS_TYPE kinematicsType()
{
static bool is_setup=0;
    if (!is_setup)  XYZCB_tdr_setup();
    return KINEMATICS_BOTH; // set as required
           // Note: If kinematics are identity, using KINEMATICS_BOTH
           //       may be used in order to allow a gui to display
           //       joint values in preview prior to homing
} // kinematicsType()

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

{
    (void)fflags;
    (void)iflags;
    double x_rot_point = *(haldata->x_rot_point);
    double y_rot_point = *(haldata->y_rot_point);
    double z_rot_point = *(haldata->z_rot_point);

    double dz = *(haldata->z_offset);
    double dt = *(haldata->tool_offset_z);
    

    // substitutions as used in mathematical documentation
    // including degree -> radians angle conversion
    double       sc = sin(j[3]*TO_RAD);
    double       cc = cos(j[3]*TO_RAD);
    double       sb = sin(j[4]*TO_RAD);
    double       cb = cos(j[4]*TO_RAD);

    // used to be consistent with math in the documentation
    double       px = 0;
    double       py = 0;
    double       pz = 0;
    
    #define DEFAULT_ANGLE   (60.0)
    
    // define forward kinematic models using case structure for
    // for switchable kinematics
    switch (switchkins_type) {
        case 0: // ====================== IDENTITY kinematics FORWARD ====================
            double angle = *(haldata->angleXX) * PM_PI / 180.0;
            
            pos->tran.x =   j[0] + j[1] * cos(angle)
            pos->tran.y = - j[1] * sin(angle);
            pos->tran.z =   j[2];
            pos->c      =   j[3];
            pos->b      =   j[4];
            break;
        case 1: // ========================= TCP kinematics FORWARD ======================
            px          =    j[0] - x_rot_point + j[1] * cos(angle);
            py          = - (j[1] - y_rot_point) * sin(angle);
            pz          =    j[2] - z_rot_point - dt;

            pos->tran.x =   cb * (cc * px - sc * py) + sb * pz + x_rot_point;
            pos->tran.y =   sc * px + cc * py + y_rot_point;
            pos->tran.z =  -sb * (cc * px - sc * py) + cb * pz + z_rot_point + dz + dt;
            pos->c      = j[3];
            pos->b      = j[4];
            break;
    }
    // unused coordinates:
    pos->a = 0;
    pos->u = 0;
    pos->v = 0;
    pos->w = 0;

    return 0;
} // kinematicsForward()

int kinematicsInverse(const EmcPose * pos,
                      double *j,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    (void)iflags;
    (void)fflags;
    double x_rot_point = *(haldata->x_rot_point);
    double y_rot_point = *(haldata->y_rot_point);
    double z_rot_point = *(haldata->z_rot_point);

    double dx = *(haldata->x_offset);
    double dz = *(haldata->z_offset);
    double dt = *(haldata->tool_offset_z);

    // substitutions as used in mathematical documentation
    // including degree -> radians angle conversion
    double       sc = sin(pos->c*TO_RAD);
    double       cc = cos(pos->c*TO_RAD);
    double       sb = sin(pos->b*TO_RAD);
    double       cb = cos(pos->b*TO_RAD);

    // used to be consistent with math in the documentation
    double       qx = 0;
    double       qy = 0;
    double       qz = 0;

    switch (switchkins_type) {
        case 0:// ====================== IDENTITY kinematics INVERSE =====================
            double angle = *(haldata->angleXX) * PM_PI / 180.0;
            
            j[0] =   pos->tran.x + pos->tran.y / tan(angle);
            j[1] = - pos->tran.y / sin(angle);
            j[2] =   pos->tran.z;
            j[3] =   pos->c;
            j[4] =   pos->b;
            break;
        case 1: // ========================= TCP kinematics INVERSE ======================
            qx   = pos->tran.x - x_rot_point - dx;
            qy   = pos->tran.y - y_rot_point;
            qz   = pos->tran.z - z_rot_point - dz - dt;

            // Hier integrieren wir die virtuelle Achse (pos->tran.y) wie im Identity-Fall:
            double qx_virtual = qx + qy / tan(angle);
            double qy_virtual = -qy / sin(angle);

            j[0] = cc * (cb * qx_virtual - sb * qz) + sc * qy_virtual + cb * dx - sb * dz + x_rot_point;
            j[1] = -sc * (cb * qx_virtual - sb * qz) + cc * qy_virtual + y_rot_point;
            j[2] = sb * qx_virtual + cb * qz + sb * dx + cb * dz + z_rot_point + dt;
            j[3] = pos->c;
            j[4] = pos->b;
            break;
    }

    return 0;
} // kinematicsInverse()