blocks.c

CNC 的开放码,EMC2 V2.2.8版

    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* allocate shared memory for first order limiter */
    limit1 = hal_malloc(sizeof(limit1_t));
    if (limit1 == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pin for input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit1.%d.in", num);
    retval = hal_pin_float_new(buf, HAL_IN, &(limit1->in), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit1.%d.out", num);
    retval = hal_pin_float_new(buf, HAL_OUT, &(limit1->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export params */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit1.%d.min", num);
    retval = hal_param_float_new(buf, HAL_RW, &(limit1->min), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit1.%d.max", num);
    retval = hal_param_float_new(buf, HAL_RW, &(limit1->max), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    /* export function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit1.%d", num);
    retval = hal_export_funct(buf, limit1_funct, limit1, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' funct export failed\n", buf);
	return -1;
    }
    /* set default parameter values */
    limit1->min = -1e20;
    limit1->max =  1e20;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_limit2(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    limit2_t *limit2;

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* allocate shared memory for second order limiter */
    limit2 = hal_malloc(sizeof(limit2_t));
    if (limit2 == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pin for input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit2.%d.in", num);
    retval = hal_pin_float_new(buf, HAL_IN, &(limit2->in), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit2.%d.out", num);
    retval = hal_pin_float_new(buf, HAL_OUT, &(limit2->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export params */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit2.%d.min", num);
    retval = hal_param_float_new(buf, HAL_RW, &(limit2->min), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit2.%d.max", num);
    retval = hal_param_float_new(buf, HAL_RW, &(limit2->max), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit2.%d.maxv", num);
    retval = hal_param_float_new(buf, HAL_RW, &(limit2->maxv), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    /* export function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit2.%d", num);
    retval = hal_export_funct(buf, limit2_funct, limit2, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' funct export failed\n", buf);
	return -1;
    }
    /* set default parameter values */
    limit2->min = -1e20;
    limit2->max =  1e20;
    limit2->maxv = 1e20;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_limit3(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    limit3_t *limit3;

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* allocate shared memory for third order limiter */
    limit3 = hal_malloc(sizeof(limit3_t));
    if (limit3 == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pin for input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit3.%d.in", num);
    retval = hal_pin_float_new(buf, HAL_IN, &(limit3->in), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit3.%d.out", num);
    retval = hal_pin_float_new(buf, HAL_OUT, &(limit3->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export params */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit3.%d.min", num);
    retval = hal_param_float_new(buf, HAL_RW, &(limit3->min), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit3.%d.max", num);
    retval = hal_param_float_new(buf, HAL_RW, &(limit3->max), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit3.%d.maxv", num);
    retval = hal_param_float_new(buf, HAL_RW, &(limit3->maxv), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit3.%d.maxa", num);
    retval = hal_param_float_new(buf, HAL_RW, &(limit3->maxa), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' param export failed\n", buf);
	return retval;
    }
    /* export function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "limit3.%d", num);
    retval = hal_export_funct(buf, limit3_funct, limit3, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' funct export failed\n", buf);
	return -1;
    }
    /* set default parameter values */
    limit3->min = -1e20;
    limit3->max =  1e20;
    limit3->maxv = 1e20;
    limit3->maxa = 1e20;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_estop(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    estop_t *estop;

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* allocate shared memory for an estop latch */
    estop = hal_malloc(sizeof(estop_t));
    if (estop == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pins for inputs */
    rtapi_snprintf(buf, HAL_NAME_LEN, "estop.%d.ok-in", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(estop->ok_in), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "estop.%d.fault-in", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(estop->fault_in), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "estop.%d.reset", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(estop->reset), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pins for outputs */
    rtapi_snprintf(buf, HAL_NAME_LEN, "estop.%d.ok-out", num);
    retval = hal_pin_bit_new(buf, HAL_OUT, &(estop->ok_out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "estop.%d.fault-out", num);
    retval = hal_pin_bit_new(buf, HAL_OUT, &(estop->fault_out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "estop.%d.watchdog", num);
    retval = hal_pin_bit_new(buf, HAL_OUT, &(estop->wd), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "estop.%d", num);
    retval = hal_export_funct(buf, estop_funct, estop, 0, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' funct export failed\n", buf);
	return -1;
    }
    /* initialize internal vars */
    estop->old_reset = 1;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_not(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    not_t *not;

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* allocate shared memory for 1-input logical not */
    not = hal_malloc(sizeof(not_t));
    if (not == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pin for input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "not.%d.in", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(not->in), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "not.%d.out", num);
    retval = hal_pin_bit_new(buf, HAL_OUT, &(not->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "not.%d", num);
    retval = hal_export_funct(buf, not_funct, not, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' funct export failed\n", buf);
	return -1;
    }
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_and2(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    and2_t *and2;

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* allocate shared memory for 2-input logical and */
    and2 = hal_malloc(sizeof(and2_t));
    if (and2 == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pins for inputs */
    rtapi_snprintf(buf, HAL_NAME_LEN, "and2.%d.in0", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(and2->in0), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "and2.%d.in1", num);
    retval = hal_pin_bit_new(buf, HAL_IN, &(and2->in1), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "and2.%d.out", num);
    retval = hal_pin_bit_new(buf, HAL_OUT, &(and2->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export function */
    rtapi_snprintf(buf, HAL_NAME_LEN, "and2.%d", num);
    retval = hal_export_funct(buf, and2_funct, and2, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' funct export failed\n", buf);
	return -1;
    }
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_or2(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    or2_t *or2;

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* allocate shared memory for 2-input logical or */
    or2 = hal_malloc(sizeof