blocks.c

CNC 的开放码,EMC2 V2.2.8版

    }
    max_v = limit3->old_v + limit3->maxa * dt;
    if ( max_v > limit3->maxv ) {
	max_v = limit3->maxv;
    }
    min_out = limit3->old_out + min_v * dt;
    max_out = limit3->old_out + max_v * dt;
    if ( ( in >= min_out ) && ( in <= max_out ) && ( in_v >= min_v ) && ( in_v <= max_v ) ) {
	/* we can follow the command without hitting a limit */
	out = in;
	limit3->old_v = ( out - limit3->old_out ) / dt;
    } else {
	/* can't follow commanded path while obeying limits */ 
	/* determine which way we need to ramp to match v */
	if ( in_v > limit3->old_v ) {
	    ramp_a = limit3->maxa;
	} else {
	    ramp_a = -limit3->maxa;
	}
	/* determine how long the match would take */
	match_time = ( in_v - limit3->old_v ) / ramp_a;
	/* where we will be at the end of the match */
	avg_v = ( in_v + limit3->old_v + ramp_a * dt ) * 0.5;
	est_out = limit3->old_out + avg_v * match_time;
	/* calculate the expected command position at that time */
	est_in = limit3->old_in + in_v * match_time;
	/* calculate position error at that time */
	err = est_out - est_in;
	/* calculate change in final position if we ramp in the
	   opposite direction for one period */
	dv = -2.0 * ramp_a * dt;
	dp = dv * match_time;
	
	/* decide what to do */
	if ( fabs(err+dp*2.0) < fabs(err) ) {
	    ramp_a = -ramp_a;
	}
		
		
	if ( ramp_a < 0.0 ) {
	    out = min_out;
	    limit3->old_v = min_v;
	} else {
	    out = max_out;
	    limit3->old_v = max_v;
	}
    }    
    limit3->old_out = out;
    limit3->old_in = in;
    *(limit3->out) = out;
}

/* this module sets 'out' true when 'in' is true _and_ it sees
   a rising edge on 'reset'.  If 'in' goes false, it sets 'out'
   false.  It also toggles 'watchdog' constantly, which should
   be used for a charge pump or other such circuit.  Note that
   this block should _not_ be relied on to turn off 'out', 
   instead, 'in' should be daisy-chained through 'out' in
   hardware, and this module is only used to prevent a restart
   if/when 'in' comes back on.
*/

static void estop_funct(void *arg, long period)
{
    estop_t *estop;

    /* point to block data */
    estop = (estop_t *) arg;

    /* check inputs */
    if (( *(estop->ok_in) != 0 ) && ( *(estop->fault_in) == 0 )) {
	/* no fault conditions, check for reset edge */
	if (( *(estop->reset) != 0 ) && ( estop->old_reset == 0 )) {
	    /* got a rising edge, indicate "OK" on outputs */
	    *(estop->ok_out) = 1;
	    *(estop->fault_out) = 0;
	}
	/* toggle watchdog */
	if ( *(estop->wd) == 0 ) {
	    *(estop->wd) = 1;
	} else {
	    *(estop->wd) = 0;
	}
    } else {
	/* fault condition exists, trip */
	*(estop->ok_out) = 0;
	*(estop->fault_out) = 1;
    }
    /* store state of reset input for next pass (for edge detect) */
    estop->old_reset = *(estop->reset);
}


static void not_funct(void *arg, long period)
{
    not_t *not;

    /* point to block data */
    not = (not_t *) arg;
    /* calculate output */
    *(not->out) = ! *(not->in);
}

static void and2_funct(void *arg, long period)
{
    and2_t *and2;

    /* point to block data */
    and2 = (and2_t *) arg;
    /* calculate output */
    *(and2->out) = *(and2->in0) && *(and2->in1);
}

static void or2_funct(void *arg, long period)
{
    or2_t *or2;

    /* point to block data */
    or2 = (or2_t *) arg;
    /* calculate output */
    *(or2->out) = *(or2->in0) || *(or2->in1);
}

static void scale_funct(void *arg, long period)
{
    scale_t *scale;

    /* point to block data */
    scale = (scale_t *) arg;
    /* calculate output */
    *(scale->out) = *(scale->in) * scale->gain + scale->offset;
}

static void lowpass_funct(void *arg, long period)
{
    lowpass_t *lowpass;

    /* point to block data */
    lowpass = (lowpass_t *) arg;
    /* calculate output */
    *(lowpass->out) += (*(lowpass->in) - *(lowpass->out)) * lowpass->gain;
}

static void match8_funct(void *arg, long period)
{
    match8_t *match8;
    int n;
    hal_bit_t a, b, tmp;

    /* point to block data */
    match8 = (match8_t *) arg;
    /* calculate output, starting with cascade/enable input */
    if ( *(match8->in) != 0 ) {
	/* input true, process a and b */
	tmp = 1;
	n = 0;
    } else {
	/* input false, ignore a and b */
	tmp = 0;
	n = 8;
    }
    /* test all bits, exit loop as soon as mismatch found */
    while ( n < 8 ) {
	a = *(match8->a[n]);
	b = *(match8->b[n]);
	if ((( a == 0 ) && ( b != 0 )) || (( a != 0 ) && ( b == 0 ))) {
	    tmp = 0;
	    n = 8;
	}
	n++;
    }
    /* write output */
    *(match8->out) = tmp;
}

static void minmax_funct(void *arg, long period)
{
    minmax_t *minmax;
    float in;

    /* point to block data */
    minmax = (minmax_t *) arg;
    /* get input to local */
    in = *(minmax->in);
    if ( *(minmax->reset) ) {
	/* when reset, min and max both track input */
	*(minmax->min) = in;
	*(minmax->max) = in;
    } else {
	if ( in < *(minmax->min) ) {
	    *(minmax->min) = in;
	}
	if ( in > *(minmax->max) ) {
	    *(minmax->max) = in;
	}
    }
}

/***********************************************************************
*                   LOCAL FUNCTION DEFINITIONS                         *
************************************************************************/

static int export_constant(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    constant_t *constant;

    /* 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 constant */
    constant = hal_malloc(sizeof(constant_t));
    if (constant == 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: hal_malloc() failed\n");
	return -1;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "constant.%d.out", num);
    retval = hal_pin_float_new(buf, HAL_OUT, &(constant->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export param for value */
    rtapi_snprintf(buf, HAL_NAME_LEN, "constant.%d.value", num);
    retval = hal_param_float_new(buf, HAL_RW, &(constant->value), 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, "constant.%d", num);
    retval = hal_export_funct(buf, constant_funct, constant, 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 */
    constant->value = 1.0;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_wcomp(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    wcomp_t *wcomp;

    /* 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 window comparator */
    wcomp = hal_malloc(sizeof(wcomp_t));
    if (wcomp == 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, "wcomp.%d.in", num);
    retval = hal_pin_float_new(buf, HAL_IN, &(wcomp->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, "wcomp.%d.out", num);
    retval = hal_pin_bit_new(buf, HAL_OUT, &(wcomp->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export params for min and max */
    rtapi_snprintf(buf, HAL_NAME_LEN, "wcomp.%d.min", num);
    retval = hal_param_float_new(buf, HAL_RW, &(wcomp->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, "wcomp.%d.max", num);
    retval = hal_param_float_new(buf, HAL_RW, &(wcomp->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, "wcomp.%d", num);
    retval = hal_export_funct(buf, wcomp_funct, wcomp, 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 */
    wcomp->min = -1.0;
    wcomp->max = 1.0;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_comp(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    comp_t *comp;

    /* 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 comparator */
    comp = hal_malloc(sizeof(comp_t));
    if (comp == 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, "comp.%d.in0", num);
    retval = hal_pin_float_new(buf, HAL_IN, &(comp->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, "comp.%d.in1", num);
    retval = hal_pin_float_new(buf, HAL_IN, &(comp->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, "comp.%d.out", num);
    retval = hal_pin_bit_new(buf, HAL_OUT, &(comp->out), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "BLOCKS: ERROR: '%s' pin export failed\n", buf);
	return retval;
    }
    /* export params for hystersis */
    rtapi_snprintf(buf, HAL_NAME_LEN, "comp.%d.hyst", num);
    retval = hal_param_float_new(buf, HAL_RW, &(comp->hyst), 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, "comp.%d", num);
    retval = hal_export_funct(buf, comp_funct, comp, 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 */
    comp->hyst = 0.0;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}

static int export_hypot(int num)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];
    hypot_t *h;

    /* 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 hypotenuse calculator */
    h = hal_malloc(sizeof(hypot_t));
    if (h == 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, "hypot.%d.in0", num);
    retval = hal_pin_float_new(buf, HAL_IN, &(h->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, "hypot.%d.in1", num);
    retval = hal_pin_float_new(buf, HAL_IN, &(h->in1), comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,