blocks.c

CNC 的开放码,EMC2 V2.2.8版

		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_sum2(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d 2-input summers\n", sum2);
    }
    /* allocate and export 2 input multipliers */
    if (mult2 > 0) {
	for (n = 0; n < mult2; n++) {
	    if (export_mult2(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_mult2(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d 2-input multipliers\n", mult2);
    }
    /* allocate and export integrators */
    if (integ > 0) {
	for (n = 0; n < integ; n++) {
	    if (export_integ(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_integ(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d 2 integrators\n", integ);
    }
    /* allocate and export differentiators */
    if (ddt > 0) {
	for (n = 0; n < ddt; n++) {
	    if (export_ddt(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_ddt(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d differentiators\n", ddt);
    }
    /* allocate and export first order limiters */
    if (limit1 > 0) {
	for (n = 0; n < limit1; n++) {
	    if (export_limit1(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_limit1(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d first order limiters\n", limit1);
    }
    /* allocate and export second order limiters */
    if (limit2 > 0) {
	for (n = 0; n < limit2; n++) {
	    if (export_limit2(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_limit2(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d second order limiters\n", limit2);
    }
    /* allocate and export third order limiters */
    if (limit3 > 0) {
	for (n = 0; n < limit3; n++) {
	    if (export_limit3(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_limit3(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d third order limiters\n", limit3);
    }
    /* allocate and export estop latch blocks */
    if (n_estop > 0) {
	for (n = 0; n < n_estop; n++) {
	    if (export_estop(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_estop(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d estop latch blocks\n", n_estop);
    }
    /* allocate and export 1 input logical nots */
    if (not > 0) {
	for (n = 0; n < not; n++) {
	    if (export_not(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_not(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d logical inverters\n", not);
    }
    /* allocate and export 2 input logical ands */
    if (and2 > 0) {
	for (n = 0; n < and2; n++) {
	    if (export_and2(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_and2(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d 2-input logical ands\n", and2);
    }
    /* allocate and export 2 input logical ors */
    if (or2 > 0) {
	for (n = 0; n < or2; n++) {
	    if (export_or2(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_or2(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d 2-input logical ors\n", or2);
    }
    /* allocate and export scalers */
    if (scale > 0) {
	for (n = 0; n < scale; n++) {
	    if (export_scale(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_scale(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d scalers\n", scale);
    }
    /* allocate and export lowpass filters */
    if (lowpass > 0) {
	for (n = 0; n < lowpass; n++) {
	    if (export_lowpass(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_lowpass(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d lowpass filters\n", lowpass);
    }
    /* allocate and export binary match detectors */
    if (match8 > 0) {
	for (n = 0; n < match8; n++) {
	    if (export_match8(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_match8(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d match detectors\n", match8);
    }
    /* allocate and export min/max blocks */
    if (minmax > 0) {
	for (n = 0; n < minmax; n++) {
	    if (export_minmax(n) != 0) {
		rtapi_print_msg(RTAPI_MSG_ERR,
		    "BLOCKS: ERROR: export_minmax(%d) failed\n", n);
		hal_exit(comp_id);
		return -1;
	    }
	}
	rtapi_print_msg(RTAPI_MSG_INFO,
	    "BLOCKS: installed %d min/max blocks\n", minmax);
    }
    hal_ready(comp_id);
    return 0;
}

void rtapi_app_exit(void)
{
    hal_exit(comp_id);
}

/***********************************************************************
*                     REALTIME BLOCK FUNCTIONS                         *
************************************************************************/

static void constant_funct(void *arg, long period)
{
    constant_t *constant;

    /* point to block data */
    constant = (constant_t *) arg;
    /* calculate output */
    *(constant->out) = constant->value;
}

static void wcomp_funct(void *arg, long period)
{
    wcomp_t *wcomp;
    float tmp;

    /* point to block data */
    wcomp = (wcomp_t *) arg;
    /* calculate output */
    tmp = *(wcomp->in);
    if ((wcomp->min < tmp) && (tmp < wcomp->max)) {
	*(wcomp->out) = 1;
    } else {
	*(wcomp->out) = 0;
    }
}

static void comp_funct(void *arg, long period)
{
    comp_t *comp;
    float tmp;

    /* point to block data */
    comp = (comp_t *) arg;
    /* calculate output */
    tmp = *(comp->in1) - *(comp->in0);
    if (*(comp->out)) {
	if (tmp < -comp->hyst/2.) {
	    *(comp->out) = 0;
	}
    } else {
	if (tmp > comp->hyst/2.) {
	    *(comp->out) = 1;
	}
    }
}

static void hypot_funct(void *arg, long period)
{
    double a, b, c;
    hypot_t *hypot;

    /* point to block data */
    hypot = (hypot_t *) arg;

    /* grab inputs to locals */
    a = *(hypot->in0);
    b = *(hypot->in1);
    c = *(hypot->in2);

    /* calculate output */
    *(hypot->out) = sqrt(a*a + b*b + c*c);
}

static void mux2_funct(void *arg, long period)
{
    mux2_t *mux2;

    /* point to block data */
    mux2 = (mux2_t *) arg;
    /* calculate output */
    if (*(mux2->sel)) {
	*(mux2->out) = *(mux2->in1);
    } else {
	*(mux2->out) = *(mux2->in0);
    }
}

static void mux4_funct(void *arg, long period)
{
    mux4_t *mux4;

    /* point to block data */
    mux4 = (mux4_t *) arg;
    /* calculate output */
    if (*(mux4->sel1)) {
	if (*(mux4->sel0)) {
	    *(mux4->out) = *(mux4->in3);
	} else {
	    *(mux4->out) = *(mux4->in2);
	}
    } else {
	if (*(mux4->sel0)) {
	    *(mux4->out) = *(mux4->in1);
	} else {
	    *(mux4->out) = *(mux4->in0);
	}
    }
}

static void sum2_funct(void *arg, long period)
{
    sum2_t *sum2;

    /* point to block data */
    sum2 = (sum2_t *) arg;
    /* calculate output */
    *(sum2->out) = *(sum2->in0) * sum2->gain0 + *(sum2->in1) * sum2->gain1;
}

static void mult2_funct(void *arg, long period)
{
    mult2_t *mult2;

    /* point to block data */
    mult2 = (mult2_t *) arg;
    /* calculate output */
    *(mult2->out) = *(mult2->in0) * *(mult2->in1);
}

static void integ_funct(void *arg, long period)
{
    integ_t *integ;

    /* point to block data */
    integ = (integ_t *) arg;
    /* calculate output */
    *(integ->out) += *(integ->in) * (period * 0.000000001);
}

static void ddt_funct(void *arg, long period)
{
    ddt_t *ddt;
    float tmp;

    /* point to block data */
    ddt = (ddt_t *) arg;
    /* calculate output */
    tmp = *(ddt->in);
    *(ddt->out) = (tmp - ddt->old) / (period * 0.000000001);
    ddt->old = tmp;
}

static void limit1_funct(void *arg, long period)
{
    limit1_t *limit1;
    double out;

    /* point to block data */
    limit1 = (limit1_t *) arg;
    /* apply first order limit */
    out = *(limit1->in);
    if ( out < limit1->min ) {
	out = limit1->min;
    }
    if ( out > limit1->max ) {
	out = limit1->max;
    }
    *(limit1->out) = out;
}

static void limit2_funct(void *arg, long period)
{
    limit2_t *limit2;
    double out, delta, tmp;

    /* point to block data */
    limit2 = (limit2_t *) arg;
    /* apply first order limit */
    out = *(limit2->in);
    if ( out < limit2->min ) {
	out = limit2->min;
    }
    if ( out > limit2->max ) {
	out = limit2->max;
    }
    /* apply second order limit */
    delta = limit2->maxv * (period * 0.000000001);
    tmp = limit2->old_out - delta;
    if ( out < tmp ) {
	out = tmp;
    }
    tmp = limit2->old_out + delta;
    if ( out > tmp ) {
	out = tmp;
    }
    limit2->old_out = out;
    *(limit2->out) = out;
}

static void limit3_funct(void *arg, long period)
{
    limit3_t *limit3;
    double in, out, dt, in_v, min_v, max_v, ramp_a, avg_v, err, dv, dp;
    double min_out, max_out, match_time, est_in, est_out;

    est_in = est_out = match_time = 0;
    
    /* point to block data */
    limit3 = (limit3_t *) arg;
    /* apply first order limit */
    in = *(limit3->in);
    if ( in < limit3->min ) {
	in = limit3->min;
    }
    if ( in > limit3->max ) {
	in = limit3->max;
    }
    /* calculate input derivative */
    dt = period * 0.000000001;
    in_v = (in - limit3->old_in) / dt;
    /* determine v and out that can be reached in one period */
    min_v = limit3->old_v - limit3->maxa * dt;
    if ( min_v < -limit3->maxv ) {
	min_v = -limit3->maxv;