Advanced Search

static bool is_ready=0;
int kinematicsForward(const double *j,
                      EmcPose * pos,
                      const KINEMATICS_FORWARD_FLAGS * fflags,
                      KINEMATICS_INVERSE_FLAGS * iflags)
{
    static bool gave_msg;
    // define forward kinematic models using case structure for
    // for switchable kinematics
    switch (switchkins_type) {
        case 0:
            pos->tran.x = j[0];
            pos->tran.y = j[1];
            pos->tran.z = j[2];
            pos->a      = j[3];
            break;
        case 1:
            pos->tran.x = j[2];
            pos->tran.y = -j[1];
            pos->tran.z = j[0];
            pos->a      = j[3];
            break;
    }
    // unused coordinates:
    pos->b = 0;
    pos->c = 0;
    pos->u = 0;
    pos->v = 0;
    pos->w = 0;

    if (*haldata->in && !is_ready && !gave_msg) {
       rtapi_print_msg(RTAPI_MSG_ERR,
                       "%s the 'in' pin not echoed until Inverse called\n",
                      __FILE__);
       gave_msg=1;
    }
    return 0;
} // kinematicsForward()

int kinematicsInverse(const EmcPose * pos,
                      double *j,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH

    // Update the kinematic joints specified by the
    // [KINS]JOINTS setting (4 required for this template).
    // define forward kinematic models using case structure for
    // for switchable kinematics
    switch (switchkins_type) {
        case 0:
            j[0] = pos->tran.x;
            j[1] = pos->tran.y;
            j[2] = pos->tran.z;
            j[3] = pos->a;
            break;
        case 1:
            j[2] = pos->tran.x;
            j[1] = -pos->tran.y;
            j[0] = pos->tran.z;
            j[3] = pos->a;
            break;
    }

    //example hal pin update (homing reqd before kinematicsInverse)
    *haldata->out = *haldata->in; //dereference
    //read from param example: *haldata->out = haldata->param_rw;

    return 0;
} // kinematicsInverse()

int kinematicsInverse(const EmcPose * pos,
                      double *j,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH

    // Update the kinematic joints specified by the
    // [KINS]JOINTS setting (3 required for this template).
    // Maximum number of joints is defined in include file:
    //         emcmotcfg:EMCMOT_MAX_JOINTS (typ 16)
    // Some joints may be claimed as 'extrajoints' and
    // do not participate in kinematics calculations --
    // $ man motion for more info
    j[0] = - pos->tran.x; // joint 0
    j[1] = pos->tran.y; // joint 1
    j[2] = pos->tran.z; // joint 2

    //example hal pin update (homing reqd before kinematicsInverse)
    *haldata->out = *haldata->in; //dereference
    //read from param example: *haldata->out = haldata->param_rw;

    return 0;
} // kinematicsInverse()