stepgen.c

CNC 的开放码,EMC2 V2.2.8版

    }

    /* point at stepgen data */
    stepgen = arg;

    /* loop thru generators */
    for (n = 0; n < num_chan; n++) {
	/* check for scale change */
	if (stepgen->pos_scale != stepgen->old_scale) {
	    /* get ready to detect future scale changes */
	    stepgen->old_scale = stepgen->pos_scale;
	    /* validate the new scale value */
	    if ((stepgen->pos_scale < 1e-20)
		&& (stepgen->pos_scale > -1e-20)) {
		/* value too small, divide by zero is a bad thing */
		stepgen->pos_scale = 1.0;
	    }
	    /* we will need the reciprocal, and the accum is fixed point with
	       fractional bits, so we precalc some stuff */
	    stepgen->scale_recip = (1.0 / (1L << PICKOFF)) / stepgen->pos_scale;
	}
	if ( newperiod ) {
	    /* period changed, force recalc of timing parameters */
	    stepgen->old_step_len = ~0;
	    stepgen->old_step_space = ~0;
	    stepgen->old_dir_hold_dly = ~0;
	    stepgen->old_dir_setup = ~0;
	}
	/* process timing parameters */
	if ( stepgen->step_len != stepgen->old_step_len ) {
	    /* must be non-zero */
	    if ( stepgen->step_len == 0 ) {
		stepgen->step_len = 1;
	    }
	    /* make integer multiple of periodns */
	    stepgen->old_step_len = ulceil(stepgen->step_len, periodns);
	    stepgen->step_len = stepgen->old_step_len;
	}
	if ( stepgen->step_space != stepgen->old_step_space ) {
	    /* make integer multiple of periodns */
	    stepgen->old_step_space = ulceil(stepgen->step_space, periodns);
	    stepgen->step_space = stepgen->old_step_space;
	}
	if ( stepgen->dir_setup != stepgen->old_dir_setup ) {
	    /* make integer multiple of periodns */
	    stepgen->old_dir_setup = ulceil(stepgen->dir_setup, periodns);
	    stepgen->dir_setup = stepgen->old_dir_setup;
	}
	if ( stepgen->dir_hold_dly != stepgen->old_dir_hold_dly ) {
	    if ( (stepgen->dir_hold_dly + stepgen->dir_setup) == 0 ) {
		/* dirdelay must be non-zero step types 0 and 1 */
		if ( stepgen->step_type < 2 ) {
		    stepgen->dir_hold_dly = 1;
		}
	    }
	    stepgen->old_dir_hold_dly = ulceil(stepgen->dir_hold_dly, periodns);
	    stepgen->dir_hold_dly = stepgen->old_dir_hold_dly;
	}
	/* test for disabled stepgen */
	if (*stepgen->enable == 0) {
	    /* disabled: keep updating old_pos_cmd (if in pos ctrl mode) */
	    if ( stepgen->pos_mode ) {
		stepgen->old_pos_cmd = *stepgen->pos_cmd * stepgen->pos_scale;
	    }
	    /* set velocity to zero */
	    stepgen->freq = 0;
	    stepgen->addval = 0;
	    stepgen->target_addval = 0;
	    /* and skip to next one */
	    stepgen++;
	    continue;
	}
	/* calculate frequency limit */
	min_step_period = stepgen->step_len + stepgen->step_space;
	max_freq = 1.0 / (min_step_period * 0.000000001);
	/* check for user specified frequency limit parameter */
	if (stepgen->maxvel <= 0.0) {
	    /* set to zero if negative */
	    stepgen->maxvel = 0.0;
	} else {
	    /* parameter is non-zero, compare to max_freq */
	    desired_freq = stepgen->maxvel * fabs(stepgen->pos_scale);
	    if (desired_freq > max_freq) {
		/* parameter is too high, complain about it */
		if(!stepgen->printed_error) {
		    rtapi_print_msg(RTAPI_MSG_ERR,
			"STEPGEN: Channel %d: The requested maximum velocity of %d steps/sec is too high.\n",
			n, (int)desired_freq);
		    rtapi_print_msg(RTAPI_MSG_ERR,
			"STEPGEN: The maximum possible frequency is %d steps/second\n",
			(int)max_freq);
		    stepgen->printed_error = 1;
		}
		/* parameter is too high, limit it */
		stepgen->maxvel = max_freq / fabs(stepgen->pos_scale);
	    } else {
		/* lower max_freq to match parameter */
		max_freq = stepgen->maxvel * fabs(stepgen->pos_scale);
	    }
	}
	/* set internal accel limit to its absolute max, which is
	   zero to full speed in one thread period */
	max_ac = max_freq * recip_dt;
	/* check for user specified accel limit parameter */
	if (stepgen->maxaccel <= 0.0) {
	    /* set to zero if negative */
	    stepgen->maxaccel = 0.0;
	} else {
	    /* parameter is non-zero, compare to max_ac */
	    if ((stepgen->maxaccel * fabs(stepgen->pos_scale)) > max_ac) {
		/* parameter is too high, lower it */
		stepgen->maxaccel = max_ac / fabs(stepgen->pos_scale);
	    } else {
		/* lower limit to match parameter */
		max_ac = stepgen->maxaccel * fabs(stepgen->pos_scale);
	    }
	}
	/* at this point, all scaling, limits, and other parameter
	   changes have been handled - time for the main control */
	if ( stepgen->pos_mode ) {
	    /* calculate position command in counts */
	    pos_cmd = *stepgen->pos_cmd * stepgen->pos_scale;
	    /* calculate velocity command in counts/sec */
	    vel_cmd = (pos_cmd - stepgen->old_pos_cmd) * recip_dt;
	    stepgen->old_pos_cmd = pos_cmd;
	    /* 'accum' is a long long, and its remotely possible that
	       make_pulses could change it half-way through a read.
	       So we have a crude atomic read routine */
	    do {
		accum_a = stepgen->accum;
		accum_b = stepgen->accum;
	    } while ( accum_a != accum_b );
	    /* convert from fixed point to double, after subtracting
	       the one-half step offset */
	    curr_pos = (accum_a-(1<< (PICKOFF-1))) * (1.0 / (1L << PICKOFF));
	    /* get velocity in counts/sec */
	    curr_vel = stepgen->freq;
	    /* At this point we have good values for pos_cmd, curr_pos,
	       vel_cmd, curr_vel, max_freq and max_ac, all in counts,
	       counts/sec, or counts/sec^2.  Now we just have to do
	       something useful with them. */
	    /* determine which way we need to ramp to match velocity */
	    if (vel_cmd > curr_vel) {
		match_ac = max_ac;
	    } else {
		match_ac = -max_ac;
	    }
	    /* determine how long the match would take */
	    match_time = (vel_cmd - curr_vel) / match_ac;
	    /* calc output position at the end of the match */
	    avg_v = (vel_cmd + curr_vel) * 0.5;
	    est_out = curr_pos + avg_v * match_time;
	    /* calculate the expected command position at that time */
	    est_cmd = pos_cmd + vel_cmd * (match_time - 1.5 * dt);
	    /* calculate error at that time */
	    est_err = est_out - est_cmd;
	    if (match_time < dt) {
		/* we can match velocity in one period */
		if (fabs(est_err) < 0.0001) {
		    /* after match the position error will be acceptable */
		    /* so we just do the velocity match */
		    new_vel = vel_cmd;
		} else {
		    /* try to correct position error */
		    new_vel = vel_cmd - 0.5 * est_err * recip_dt;
		    /* apply accel limits */
		    if (new_vel > (curr_vel + max_ac * dt)) {
			new_vel = curr_vel + max_ac * dt;
		    } else if (new_vel < (curr_vel - max_ac * dt)) {
			new_vel = curr_vel - max_ac * dt;
		    }
		}
	    } else {
		/* calculate change in final position if we ramp in the
		   opposite direction for one period */
		dv = -2.0 * match_ac * dt;
		dp = dv * match_time;
		/* decide which way to ramp */
		if (fabs(est_err + dp * 2.0) < fabs(est_err)) {
		    match_ac = -match_ac;
		}
		/* and do it */
		new_vel = curr_vel + match_ac * dt;
	    }
	    /* apply frequency limit */
	    if (new_vel > max_freq) {
		new_vel = max_freq;
	    } else if (new_vel < -max_freq) {
		new_vel = -max_freq;
	    }
	    /* end of position mode */
	} else {
	    /* velocity mode is simpler */
	    /* calculate velocity command in counts/sec */
	    vel_cmd = *(stepgen->vel_cmd) * stepgen->pos_scale;
	    /* apply frequency limit */
	    if (vel_cmd > max_freq) {
		vel_cmd = max_freq;
	    } else if (vel_cmd < -max_freq) {
		vel_cmd = -max_freq;
	    }
	    /* calc max change in frequency in one period */
	    dv = max_ac * dt;
	    /* apply accel limit */
	    if ( vel_cmd > (stepgen->freq + dv) ) {
		new_vel = stepgen->freq + dv;
	    } else if ( vel_cmd < (stepgen->freq - dv) ) {
		new_vel = stepgen->freq - dv;
	    } else {
		new_vel = vel_cmd;
	    }
	    /* end of velocity mode */
	}
	stepgen->freq = new_vel;
	/* calculate new addval */
	stepgen->target_addval = stepgen->freq * freqscale;
	/* calculate new deltalim */
	stepgen->deltalim = max_ac * accelscale;
	/* move on to next channel */
	stepgen++;
    }
    /* done */
}

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

static int export_stepgen(int num, stepgen_t * addr, int step_type, int pos_mode)
{
    int n, retval, msg;

    /* 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 param variable for raw counts */
    retval = hal_param_s32_newf(HAL_RO, &(addr->rawcount), comp_id,
	"stepgen.%d.rawcounts", num);
    if (retval != 0) { return retval; }
    /* export pin for counts captured by update() */
    retval = hal_pin_s32_newf(HAL_OUT, &(addr->count), comp_id,
	"stepgen.%d.counts", num);
    if (retval != 0) { return retval; }
    /* export parameter for position scaling */
    retval = hal_param_float_newf(HAL_RW, &(addr->pos_scale), comp_id,
	"stepgen.%d.position-scale", num);
    if (retval != 0) { return retval; }
    /* export pin for command */
    if ( pos_mode ) {
	retval = hal_pin_float_newf(HAL_IN, &(addr->pos_cmd), comp_id,
	    "stepgen.%d.position-cmd", num);
    } else {
	retval = hal_pin_float_newf(HAL_IN, &(addr->vel_cmd), comp_id,
	    "stepgen.%d.velocity-cmd", num);
    }
    if (retval != 0) { return retval; }
    /* export pin for enable command */
    retval = hal_pin_bit_newf(HAL_IN, &(addr->enable), comp_id,
	"stepgen.%d.enable", num);
    if (retval != 0) { return retval; }
    /* export pin for scaled position captured by update() */
    retval = hal_pin_float_newf(HAL_OUT, &(addr->pos_fb), comp_id,
	"stepgen.%d.position-fb", num);
    if (retval != 0) { return retval; }
    /* export param for scaled velocity (frequency in Hz) */
    retval = hal_param_float_newf(HAL_RO, &(addr->freq), comp_id,
	"stepgen.%d.frequency", num);
    if (retval != 0) { return retval; }
    /* export parameter for max frequency */
    retval = hal_param_float_newf(HAL_RW, &(addr->maxvel), comp_id,
	"stepgen.%d.maxvel", num);
    if (retval != 0) { return retval; }
    /* export parameter for max accel/decel */
    retval = hal_param_float_newf(HAL_RW, &(addr->maxaccel), comp_id,
	"stepgen.%d.maxaccel", num);
    if (retval != 0) { return retval; }
    /* every step type uses steplen */
    retval = hal_param_u32_newf(HAL_RW, &(addr->step_len), comp_id,
	"stepgen.%d.steplen", num);
    if (retval != 0) { return retval; }
    if (step_type < 2) {
	/* step/dir and up/down use 'stepspace' */
	retval = hal_param_u32_newf(HAL_RW, &(addr->step_space),
	    comp_id, "stepgen.%d.stepspace", num);
	if (retval != 0) { return retval; }
    }
    if ( step_type == 0 ) {
	/* step/dir is the only one that uses dirsetup and dirhold */
	retval = hal_param_u32_newf(HAL_RW, &(addr->dir_setup),
	    comp_id, "stepgen.%d.dirsetup", num);
	if (retval != 0) { return retval; }
	retval = hal_param_u32_newf(HAL_RW, &(addr->dir_hold_dly),
	    comp_id, "stepgen.%d.dirhold", num);
	if (retval != 0) { return retval; }
    } else {
	/* the others use dirdelay */
	retval = hal_param_u32_newf(HAL_RW, &(addr->dir_hold_dly),
	    comp_id, "stepgen.%d.dirdelay", num);
	if (retval != 0) { return retval; }
    }
    /* export output pins */
    if ( step_type == 0 ) {
	/* step and direction */
	retval = hal_pin_bit_newf(HAL_OUT, &(addr->phase[STEP_PIN]),
	    comp_id, "stepgen.%d.step", num);
	if (retval != 0) { return retval; }
	*(addr->phase[STEP_PIN]) = 0;
	retval = hal_pin_bit_newf(HAL_OUT, &(addr->phase[DIR_PIN]),
	    comp_id, "stepgen.%d.dir", num);
	if (retval != 0) { return retval; }
	*(addr->phase[DIR_PIN]) = 0;
    } else if (step_type == 1) {
	/* up and down */
	retval = hal_pin_bit_newf(HAL_OUT, &(addr->phase[UP_PIN]),
	    comp_id, "stepgen.%d.up", num);
	if (retval != 0) { return retval; }
	*(addr->phase[UP_PIN]) = 0;
	retval = hal_pin_bit_newf(HAL_OUT, &(addr->phase[DOWN_PIN]),
	    comp_id, "stepgen.%d.down", num);
	if (retval != 0) { return retval; }
	*(addr->phase[DOWN_PIN]) = 0;
    } else {
	/* stepping types 2 and higher use a varying number of phase pins */
	addr->num_phases = num_phases_lut[step_type - 2];
	for (n = 0; n < addr->num_phases; n++) {
	    retval = hal_pin_bit_newf(HAL_OUT, &(addr->phase[n]),
		comp_id, "stepgen.%d.phase-%c", num, n + 'A');
	    if (retval != 0) { return retval; }
	    *(addr->phase[n]) = 0;
	}
    }
    /* set default parameter values */
    addr->pos_scale = 1.0;
    addr->old_scale = 0.0;
    addr->scale_recip = 0.0;
    addr->freq = 0.0;
    addr->maxvel = 0.0;
    addr->maxaccel = 0.0;
    addr->step_type = step_type;
    addr->pos_mode = pos_mode;
    /* timing parameter defaults depend on step type */
    addr->step_len = 1;
    if ( step_type < 2 ) {
	addr->step_space = 1;
    } else {
	addr->step_space = 0;
    }
    if ( step_type == 0 ) {
	addr->dir_hold_dly = 1;
	addr->dir_setup = 1;
    } else {
	addr->dir_hold_dly = 1;
	addr->dir_setup = 0;
    }
    /* set 'old' values to make update_freq validate the timing params */
    addr->old_step_len = ~0;
    addr->old_step_space = ~0;
    addr->old_dir_hold_dly = ~0;
    addr->old_dir_setup = ~0;
    if ( step_type >= 2 ) {
	/* init output stuff */
	addr->cycle_max = cycle_len_lut[step_type - 2] - 1;
	addr->lut = &(master_lut[step_type - 2][0]);
    }
    /* init the step generator core to zero output */
    addr->timer1 = 0;
    addr->timer2 = 0;
    addr->timer3 = 0;
    addr->hold_dds = 0;
    addr->addval = 0;
    /* accumulator gets a half step offset, so it will step half
       way between integer positions, not at the integer positions */
    addr->accum = 1 << (PICKOFF-1);
    addr->rawcount = 0;
    addr->curr_dir = 0;
    addr->state = 0;
    *(addr->enable) = 0;
    addr->target_addval = 0;
    addr->deltalim = 0;
    /* other init */
    addr->printed_error = 0;
    addr->old_pos_cmd = 0.0;
    /* set initial pin values */
    *(addr->count) = 0;
    *(addr->pos_fb) = 0.0;
    if ( pos_mode ) {
	*(addr->pos_cmd) = 0.0;
    } else {
	*(addr->vel_cmd) = 0.0;
    }
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}