Custom kinematics

/* userkfuncs.c: template file for a user set of
**               switchable kinematics functions.
** License GPL Version 2
**
** Example Usage (for customizing the genser-switchkins module):
** (works with rtpreempt only rtai --> Makefile needs work)
**
**        LDIR is LinuxCNC git root directory
**        UDIR is user directory (not in LinuxCNC git tree)
**  1) $ cp LDIR/src/emc/kinematics/userkfuncs.c  UDIR/my_userk.c
**  2) $ edit   UDIR/my_userk.c as required
**  3) $ source LDIR/scripts/rip-environment
**  4) For genser-switchkins module use make command line option:
**     $ cd LDIR/src
**     $ userkfuncs=UDIR/my_userk.c make && sudo make setuid
*/

// typical includes:
//#include "rtapi_math.h" // if reqd
#include "kinematics.h"
#include "hal.h"

// Add for kins based on genserkins:
// #include "genserkins.h" //includes gomath,hal

//**********************************************************************
// static local variables and functions go here

static int userk_inited = 0;
static struct udata {
    hal_s32_t *fct;
    hal_s32_t *ict;
} *udata;

//**********************************************************************
int userkKinematicsSetup(const int   comp_id,
                         const char* coordinates,
                         kparms*     kp)
{
    int res=0;
    rtapi_print("\nuserkKinematicsSetup:\n"
                  "   %s <%s> max_joints=%d allow_duplicates=%d\n\n",
                __FILE__,coordinates,
                kp->max_joints,kp->allow_duplicates);


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

    // HAL_IO used to allow resetting demo pins:
    res += hal_pin_s32_new("userk.fct", HAL_IO, &(udata->fct), comp_id);
    res += hal_pin_s32_new("userk.ict", HAL_IO, &(udata->ict), comp_id);
    if (res) goto error;

    userk_inited = 1;
    return 0; // 0 ==> OK

error:
    return -1;
}

int userkKinematicsForward(const double *joint,
                           struct EmcPose * world,
                           const KINEMATICS_FORWARD_FLAGS * fflags,
                           KINEMATICS_INVERSE_FLAGS * iflags)
{
    if (!userk_inited) {
        rtapi_print_msg(RTAPI_MSG_ERR,
             "myuserkins: not initialized\n");
        return -1;
    }

    world->tran.x = joint[1];
    world->tran.y = joint[0];
    world->tran.z = joint[2];
    world->a = joint[3];
    world->c = joint[4];

    return 0;

}

int userkKinematicsInverse(const EmcPose * pos,
                           double *joint,
                           const KINEMATICS_INVERSE_FLAGS * iflags,
                           KINEMATICS_FORWARD_FLAGS * fflags)
{

            joint[1] = pos->tran.x;
            joint[0] = pos->tran.y;
            joint[2] = pos->tran.z;
            joint[3] = pos->a;
            joint[4] = pos->c;

    return 0;
}

user@user-iMac:~/git/linuxcnc-master/src$ userkfuncs=emc/kinematics/mykins.c make
Reading 203/203 dependency files
Done reading dependencies
!!!USERKFUNCS=emc/kinematics/mykins.o
Reading 272/272 realtime dependency files
Done reading realtime dependencies
Compiling realtime emc/kinematics/mykins.c
Linking ../rtlib/genhexkins.so
Linking ../rtlib/genserkins.so
Linking ../rtlib/xyzac-trt-kins.so
Linking ../rtlib/xyzbc-trt-kins.so
Linking ../rtlib/scarakins.so
Linking ../rtlib/pumakins.so
Linking ../rtlib/5axiskins.so
You now need to run 'sudo make setuid' in order to run in place with access to hardware.