blocks.c

Source code for an Numeric Cmputer

	    }
	}
	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);
    }
    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) {
	    *(comp->out) = 0;
	}
    } else {
	if (tmp > comp->hyst) {
	    *(comp->out) = 1;
	}
    }
}

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