Advanced Search

#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;

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

static int xyzcb_millturn_setup(void) {
#define HAL_PREFIX "xyzcb_millturn"
    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_millturn 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);
    res += hal_pin_bit_new("kinstype.is-2", HAL_OUT, &;(haldata->kinstype_is_2), comp_id);

    // define default kinematics at startup for switchable kinematics
    *haldata->kinstype_is_0 = 1; // default at startup -> Milling Mode without TCP
    *haldata->kinstype_is_1 = 0; // -> Turning Mode
    *haldata->kinstype_is_2 = 0; // -> Milling Mode with 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;
            *haldata->kinstype_is_2 = 0;
            break;
        case 1:
            rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE1\n");
            *haldata->kinstype_is_0 = 0;
            *haldata->kinstype_is_1 = 1;
            *haldata->kinstype_is_2 = 0;
            break;
        case 2:
            rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE2\n");
            *haldata->kinstype_is_0 = 0;
            *haldata->kinstype_is_1 = 0;
            *haldata->kinstype_is_2 = 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;
            *haldata->kinstype_is_2 = 0;
            return -1; // FAIL
    }
    return 0; // ok
}

KINEMATICS_TYPE kinematicsType() {
    static bool is_setup = 0;
    if (!is_setup) xyzcb_millturn_setup();
    return KINEMATICS_BOTH; // set as required
}

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);

    double sc = sin(j[3] * TO_RAD); // Replaced A-axis with C-axis
    double cc = cos(j[3] * TO_RAD); // Replaced A-axis with C-axis
    double sb = sin(j[4] * TO_RAD);
    double cb = cos(j[4] * TO_RAD);

    // Define forward kinematic models using case structure for switchable kinematics
    switch (switchkins_type) {
        case 0: // Milling Mode without TCP
            pos->tran.x = j[0];
            pos->tran.y = j[1];
            pos->tran.z = j[2];
            pos->c = j[3]; // Replaced A-axis with C-axis
            pos->b = j[4]; // B-axis
            break;
        case 1: // Turning Mode
            pos->tran.x = j[2];
            pos->tran.y = -j[1]; // Invert Y-axis
            pos->tran.z = j[0];  // Swap X and Z for Turning Mode
            pos->c = j[3]; // C-axis
            pos->b = j[4]; // B-axis
            break;
        case 2: // Milling Mode with TCP
            double px = j[0] - x_rot_point;
            double py = j[1] - y_rot_point;
            double pz = j[2] - z_rot_point - dt;

            pos->tran.x = cb * px + sc * sb * py - cc * sb * pz + cb * dz + sb * dz + x_rot_point;
            pos->tran.y = cc * py + sc * pz + y_rot_point;
            pos->tran.z = sb * px - sc * cc * py + cc * cc * pz + sb * dx + cb * dz + z_rot_point + dt;
            pos->c = j[3]; // C-axis
            pos->b = j[4]; // B-axis
            break;
    }
    pos->u = 0;
    pos->v = 0;
    pos->w = 0;
    return 0;
}

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);

    double sc = sin(pos->c * TO_RAD); // Replaced A-axis with C-axis
    double cc = cos(pos->c * TO_RAD); // Replaced A-axis with C-axis
    double sb = sin(pos->b * TO_RAD);
    double cb = cos(pos->b * TO_RAD);

    double qx = 0;
    double qy = 0;
    double qz = 0;

    switch (switchkins_type) {
        case 0: // Milling Mode without TCP
            j[0] = pos->tran.x;
            j[1] = pos->tran.y;
            j[2] = pos->tran.z;
            j[3] = pos->c; // Add C-axis here
            j[4] = pos->b; // Add B-axis here
            break;
        case 1: // Turning Mode
            j[0] = pos->tran.z;  // Swapping X and Z axes for turning mode
            j[1] = -pos->tran.y; // Negating Y-axis for turning mode
            j[2] = pos->tran.x;  // Swapping X and Z axes for turning mode
            j[3] = pos->c; // Add C-axis here
            j[4] = pos->b; // Add B-axis here
            break;
        case 2: // Milling Mode with TCP
            qx = pos->tran.x - x_rot_point - dx;
            qy = pos->tran.y - y_rot_point;
            qz = pos->tran.z - z_rot_point - dz - dt;

            j[0] = cb * qx + sc * sb * qy - cc * sb * qz + cb * dx - sb * dz + x_rot_point;
            j[1] = cc * qy + sc * qz + y_rot_point;
            j[2] = sb * qx - sc * cc * qy + cc * cc * qz + sb * dx + cb * dz + z_rot_point + dt;
            j[3] = pos->c; // Add C-axis here
            j[4] = pos->b; // Add B-axis here
            break;
    }
    return 0;
}