stepgen.c

Source code for an Numeric Cmputer

	freqscale = ((1L << 30) * 2.0) / maxf;
	accelscale = freqscale * periodfp;
    }
    /* now recalc constants related to the period of this funct */
    /* only recalc constants if period changes */
    if (period != old_dtns) {
	/* get ready to detect future period changes */
	old_dtns = period;
	/* dT is the period of this thread, used for the position loop */
	dt = period * 0.000000001;
	/* calc the reciprocal once here, to avoid multiple divides later */
	recip_dt = 1.0 / dt;
    }
    
    /* 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 */
	    stepgen->scale_recip = 1.0 / stepgen->pos_scale;
	}
	/* test for disabled stepgen */
	if (*stepgen->enable == 0) {
	    /* disabled: keep updating old_pos_cmd */
	    stepgen->old_pos_cmd = *stepgen->pos_cmd * stepgen->pos_scale;
	    /* set velocity to zero */
	    stepgen->freq = 0;
	    stepgen->addval = 0;
	    stepgen->newaddval = 0;
	    /* and skip to next one */
	    stepgen++;
	    break;
	}
	/* calculate frequency limit */
	if (stepgen->wd.st0.step_type == 0) {
	    /* stepping type 0 limit depends on timing params */
	    max_freq =
		maxf / (stepgen->wd.st0.step_len +
			stepgen->wd.st0.step_space);
	} else if (stepgen->wd.st0.step_type == 1) {
	    /* stepping type 1 limit is thread freq / 2 */
	    max_freq = maxf * 0.5;
	} else {
	    /* all other step types can step at the thread frequency */
	    max_freq = maxf;
	}
	/* 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 */
	    if ((stepgen->maxvel * fabs(stepgen->pos_scale)) > max_freq) {
		/* parameter is too high, lower it */
		stepgen->maxvel = max_freq * fabs(stepgen->scale_recip);
	    } 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->scale_recip);
	    } else {
		/* lower limit to match parameter */
		max_ac = stepgen->maxaccel * fabs(stepgen->pos_scale);
	    }
	}
	/* 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;
	/* get current position and velocity in counts and counts/sec */
	curr_pos = stepgen->rawcount;
	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) < 1.0) {
		/* 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;
	}

	stepgen->freq = new_vel;
	/* calculate new addval */
	stepgen->newaddval = stepgen->freq * freqscale;
	/* calculate new deltalim */
	stepgen->deltalim = max_ac * accelscale;
	/* move on to next channel */
	stepgen++;
    }
    /* done */
}

static void update_pos(void *arg, long period)
{
    stepgen_t *stepgen;
    int n;

    stepgen = arg;

    for (n = 0; n < num_chan; n++) {
	/* capture raw counts to latches */
	*(stepgen->count) = stepgen->rawcount;
	/* check for change in scale value */
	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 */
	    stepgen->scale_recip = 1.0 / stepgen->pos_scale;
	}
	/* scale count to make floating point position */
	*(stepgen->pos_fb) = *(stepgen->count) * stepgen->scale_recip;
	/* move on to next channel */
	stepgen++;
    }
    /* done */
}

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

static int export_stepgen(int num, stepgen_t * addr, int step_type)
{
    int n, 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 param variable for raw counts */
    rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.rawcounts", num);
    retval = hal_param_s32_new(buf, HAL_RD, &(addr->rawcount), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for counts captured by update() */
    rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.counts", num);
    retval = hal_pin_s32_new(buf, HAL_WR, &(addr->count), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for position scaling */
    rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.position-scale", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->pos_scale), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for position command command */
    rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.position-cmd", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(addr->pos_cmd), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for enable command */
    rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.enable", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->enable), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export pin for scaled position captured by update() */
    rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.position-fb", num);
    retval = hal_pin_float_new(buf, HAL_WR, &(addr->pos_fb), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export param for scaled velocity (frequency in Hz) */
    rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.frequency", num);
    retval = hal_param_float_new(buf, HAL_RD, &(addr->freq), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for max frequency */
    rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.maxvel", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->maxvel), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameter for max accel/decel */
    rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.maxaccel", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->maxaccel), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* set default parameter values */
    addr->pos_scale = 1.0;
    addr->old_scale = 1.0;
    addr->scale_recip = 1.0;
    addr->freq = 0.0;
    addr->old_pos_cmd = 0.0;
    addr->maxvel = 0.0;
    addr->maxaccel = 0.0;
    addr->wd.st0.step_type = step_type;
    /* init the step generator core to zero output */
    addr->accum = 0;
    addr->addval = 0;
    addr->newaddval = 0;
    addr->deltalim = 0;
    addr->rawcount = 0;
    /* init the position controller */

    if (step_type == 0) {
	/* setup for stepping type 0 - step/dir */
	addr->wd.st0.need_step = 0;
	addr->wd.st0.setup_timer = 0;
	addr->wd.st0.hold_timer = 0;
	addr->wd.st0.space_timer = 0;
	addr->wd.st0.len_timer = 0;
	/* export parameters for step/dir pulse timing */
	rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.dirsetup", num);
	retval =
	    hal_param_u8_new(buf, HAL_WR, &(addr->wd.st0.dir_setup),
			     comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.dirhold", num);
	retval =
	    hal_param_u8_new(buf, HAL_WR, &(addr->wd.st0.dir_hold),
			     comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.steplen", num);
	retval =
	    hal_param_u8_new(buf, HAL_WR, &(addr->wd.st0.step_len),
			     comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.stepspace", num);
	retval =
	    hal_param_u8_new(buf, HAL_WR, &(addr->wd.st0.step_space),
			     comp_id);
	if (retval != 0) {
	    return retval;
	}
	/* init the parameters */
	addr->wd.st0.dir_setup = 1;
	addr->wd.st0.dir_hold = 1;
	addr->wd.st0.step_len = 1;
	addr->wd.st0.step_space = 1;
	/* export pins for step and direction */
	rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.step", num);
	retval =
	    hal_pin_bit_new(buf, HAL_WR, &(addr->phase[STEP_PIN]),
			    comp_id);
	if (retval != 0) {
	    return retval;
	}
	*(addr->phase[STEP_PIN]) = 0;
	rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.dir", num);
	retval =
	    hal_pin_bit_new(buf, HAL_WR, &(addr->phase[DIR_PIN]), comp_id);
	if (retval != 0) {
	    return retval;
	}
	*(addr->phase[DIR_PIN]) = 0;
    } else if (step_type == 1) {
	/* setup for stepping type 1 - pseudo-PWM */
	/* export pins for up and down */
	rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.up", num);
	retval =
	    hal_pin_bit_new(buf, HAL_WR, &(addr->phase[UP_PIN]), comp_id);
	if (retval != 0) {
	    return retval;
	}
	*(addr->phase[UP_PIN]) = 0;
	rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.down", num);
	retval =
	    hal_pin_bit_new(buf, HAL_WR, &(addr->phase[DOWN_PIN]),
			    comp_id);
	if (retval != 0) {
	    return retval;
	}
	*(addr->phase[DOWN_PIN]) = 0;
    } else {
	/* setup for stepping types 2 and higher */
	addr->wd.st2.state = 0;
	addr->wd.st2.cycle_max = cycle_len_lut[step_type - 2] - 1;
	addr->wd.st2.num_phases = num_phases_lut[step_type - 2];
	addr->wd.st2.lut = (char *) (&(master_lut[step_type - 2][0]));
	/* export pins for output phases */
	for (n = 0; n < addr->wd.st2.num_phases; n++) {
	    rtapi_snprintf(buf, HAL_NAME_LEN, "stepgen.%d.phase-%c",
			   num, n + 'A');
	    retval =
		hal_pin_bit_new(buf, HAL_WR, &(addr->phase[n]), comp_id);
	    if (retval != 0) {
		return retval;
	    }
	    *(addr->phase[n]) = 0;
	}
    }
    /* set initial pin values */
    *(addr->count) = 0;
    *(addr->pos_fb) = 0.0;
    *(addr->pos_cmd) = 0.0;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}