encoder_ratio.c

Source code for an Numeric Cmputer

	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"ENCODER_RATIO: ERROR: counter %d var export failed\n", n);
	    hal_exit(comp_id);
	    return -1;
	}
	/* init encoder pair */
	encoder_pair_array[n].master_state = 0;
	encoder_pair_array[n].slave_state = 0;
	encoder_pair_array[n].master_increment = 0;
	encoder_pair_array[n].slave_increment = 0;
	encoder_pair_array[n].raw_error = 0;
	encoder_pair_array[n].output_scale = 1.0;
	*(encoder_pair_array[n].error) = 0.0;
    }
    /* export functions */
    retval = hal_export_funct("encoder-ratio.sample", sample,
	encoder_pair_array, 0, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "ENCODER_RATIO: ERROR: sample funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    retval = hal_export_funct("encoder-ratio.update", update,
	encoder_pair_array, 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "ENCODER_RATIO: ERROR: update funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    rtapi_print_msg(RTAPI_MSG_INFO,
	"ENCODER_RATIO: installed %d encoder_ratio blocks\n", num_chan);
    /* was 'period' specified in the insmod command? */
    if (period > 0) {
	/* create a thread */
	retval = hal_create_thread("encoder-ratio.thread", period, 0);
	if (retval < 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"ENCODER_RATIO: ERROR: could not create thread\n");
	    hal_exit(comp_id);
	    return -1;
	} else {
	    rtapi_print_msg(RTAPI_MSG_INFO, "ENCODER_RATIO: created %d uS thread\n",
		period / 1000);
	}
    }
    return 0;
}

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

/***********************************************************************
*            REALTIME ENCODER COUNTING AND UPDATE FUNCTIONS            *
************************************************************************/

static void sample(void *arg, long period)
{
    encoder_pair_t *pair;
    int n;
    unsigned char state;

    pair = arg;
    for (n = 0; n < num_chan; n++) {
	/* detect transitions on master encoder */
	/* get state machine current state */
	state = pair->master_state;
	/* add input bits to state code */
	if (*(pair->master_A)) {
	    state |= SM_PHASE_A_MASK;
	}
	if (*(pair->master_B)) {
	    state |= SM_PHASE_B_MASK;
	}
	/* look up new state */
	state = lut[state & SM_LOOKUP_MASK];
	/* are we enabled? */
	if ( *(pair->enable) != 0 ) {
	    /* has an edge been detected? */
	    if (state & SM_CNT_UP_MASK) {
		pair->raw_error -= pair->master_increment;
	    } else if (state & SM_CNT_DN_MASK) {
		pair->raw_error += pair->master_increment;
	    }
	}
	/* save state machine state */
	pair->master_state = state;
	/* detect transitions on slave encoder */
	/* get state machine current state */
	state = pair->slave_state;
	/* add input bits to state code */
	if (*(pair->slave_A)) {
	    state |= SM_PHASE_A_MASK;
	}
	if (*(pair->slave_B)) {
	    state |= SM_PHASE_B_MASK;
	}
	/* look up new state */
	state = lut[state & SM_LOOKUP_MASK];
	/* has an edge been detected? */
	if (state & SM_CNT_UP_MASK) {
	    pair->raw_error += pair->slave_increment;
	} else if (state & SM_CNT_DN_MASK) {
	    pair->raw_error -= pair->slave_increment;
	}
	/* save state machine state */
	pair->slave_state = state;
	/* move on to next pair */
	pair++;
    }
    /* done */
}

static void update(void *arg, long period)
{
    encoder_pair_t *pair;
    int n;

    pair = arg;
    for (n = 0; n < num_chan; n++) {
	/* scale raw error to output pin */
	if ( pair->output_scale > 0 ) {
	    *(pair->error) = pair->raw_error / pair->output_scale;
	}
	/* update scale factors (only needed if params change, but
	   it's faster to do it every time than to detect changes.) */
	pair->master_increment = pair->master_teeth * pair->slave_ppr;
	pair->slave_increment = pair->slave_teeth * pair->master_ppr;
	pair->output_scale = pair->master_ppr * pair->slave_ppr * pair->slave_teeth;
	/* move on to next pair */
	pair++;
    }
    /* done */
}

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

static int export_encoder_pair(int num, encoder_pair_t * addr)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];

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

    /* export pins for the quadrature inputs */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-A", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->master_A), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-B", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->master_B), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-A", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->slave_A), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-B", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->slave_B), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for the enable input */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.enable", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->enable), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for output */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.error", num);
    retval = hal_pin_float_new(buf, HAL_WR, &(addr->error), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameters for config info() */
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-ppr", num);
    retval = hal_param_u32_new(buf, HAL_WR, &(addr->master_ppr), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-ppr", num);
    retval = hal_param_u32_new(buf, HAL_WR, &(addr->slave_ppr), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-teeth", num);
    retval = hal_param_u32_new(buf, HAL_WR, &(addr->master_teeth), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-teeth", num);
    retval = hal_param_u32_new(buf, HAL_WR, &(addr->slave_teeth), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}