pid.c

Source code for an Numeric Cmputer

static void calc_pid(void *arg, long period)
{
    hal_pid_t *pid;
    float tmp1;
    int enable;
    float periodfp, periodrecip;

    /* point to the data for this PID loop */
    pid = arg;
    /* precalculate some timing constants */
    periodfp = period * 0.000000001;
    periodrecip = 1.0 / periodfp;
    /* get the enable bit */
    enable = *(pid->enable);
    /* calculate the error */
    tmp1 = *(pid->command) - *(pid->feedback);
    /* store error to error pin */
    *(pid->error) = tmp1;
    /* apply error limits */
    if (pid->maxerror != 0.0) {
	if (tmp1 > pid->maxerror) {
	    tmp1 = pid->maxerror;
	} else if (tmp1 < -pid->maxerror) {
	    tmp1 = -pid->maxerror;
	}
    }
    /* apply the deadband */
    if (tmp1 > pid->deadband) {
	tmp1 -= pid->deadband;
    } else if (tmp1 < -pid->deadband) {
	tmp1 += pid->deadband;
    } else {
	tmp1 = 0;
    }
    /* do integrator calcs only if enabled */
    if (enable != 0) {
	/* if output is in limit, don't let integrator wind up */
	if ( ( tmp1 * pid->limit_state ) <= 0.0 ) {
	    /* compute integral term */
	    pid->error_i += tmp1 * periodfp;
	}
	/* apply integrator limits */
	if (pid->maxerror_i != 0.0) {
	    if (pid->error_i > pid->maxerror_i) {
		pid->error_i = pid->maxerror_i;
	    } else if (pid->error_i < -pid->maxerror_i) {
		pid->error_i = -pid->maxerror_i;
	    }
	}
    } else {
	/* not enabled, reset integrator */
	pid->error_i = 0;
    }
    /* calculate derivative term */
    pid->error_d = (tmp1 - pid->prev_error) * periodrecip;
    pid->prev_error = tmp1;
    /* apply derivative limits */
    if (pid->maxerror_d != 0.0) {
	if (pid->error_d > pid->maxerror_d) {
	    pid->error_d = pid->maxerror_d;
	} else if (pid->error_d < -pid->maxerror_d) {
	    pid->error_d = -pid->maxerror_d;
	}
    }
    /* calculate derivative of command */
    pid->cmd_d = (*(pid->command) - pid->prev_cmd) * periodrecip;
    pid->prev_cmd = *(pid->command);
    /* apply derivative limits */
    if (pid->maxcmd_d != 0.0) {
	if (pid->cmd_d > pid->maxcmd_d) {
	    pid->cmd_d = pid->maxcmd_d;
	} else if (pid->cmd_d < -pid->maxcmd_d) {
	    pid->cmd_d = -pid->maxcmd_d;
	}
    }
    /* do output calcs only if enabled */
    if (enable != 0) {
	/* calculate the output value */
	tmp1 =
	    pid->bias + pid->pgain * tmp1 + pid->igain * pid->error_i +
	    pid->dgain * pid->error_d;
	tmp1 += *(pid->command) * pid->ff0gain + pid->cmd_d * pid->ff1gain;
	/* apply output limits */
	if (pid->maxoutput != 0.0) {
	    if (tmp1 > pid->maxoutput) {
		tmp1 = pid->maxoutput;
		pid->limit_state = 1.0;
	    } else if (tmp1 < -pid->maxoutput) {
		tmp1 = -pid->maxoutput;
		pid->limit_state = -1.0;
	    } else {
		pid->limit_state = 0.0;
	    }
	}
    } else {
	/* not enabled, force output to zero */
	tmp1 = 0.0;
	pid->limit_state = 0.0;
    }
    /* write final output value to output pin */
    *(pid->output) = tmp1;
    /* done */
}

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

static int export_pid(int num, hal_pid_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 */
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.enable", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->enable), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.command", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(addr->command), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.feedback", num);
    retval = hal_pin_float_new(buf, HAL_RD, &(addr->feedback), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.error", num);
    retval = hal_pin_float_new(buf, HAL_WR, &(addr->error), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.output", num);
    retval = hal_pin_float_new(buf, HAL_WR, &(addr->output), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export parameters */
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.deadband", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->deadband), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxerror", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->maxerror), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxerrorI", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->maxerror_i), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxerrorD", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->maxerror_d), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxcmdD", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->maxcmd_d), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.bias", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->bias), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.Pgain", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->pgain), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.Igain", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->igain), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.Dgain", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->dgain), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.FF0", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->ff0gain), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.FF1", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->ff1gain), comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxoutput", num);
    retval = hal_param_float_new(buf, HAL_WR, &(addr->maxoutput), comp_id);
    if (retval != 0) {
	return retval;
    }
    /* export optional parameters */
    if (debug > 0) {
	rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.errorI", num);
	retval = hal_param_float_new(buf, HAL_RD, &(addr->error_i), comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.errorD", num);
	retval = hal_param_float_new(buf, HAL_RD, &(addr->error_d), comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.commandD", num);
	retval = hal_param_float_new(buf, HAL_RD, &(addr->cmd_d), comp_id);
	if (retval != 0) {
	    return retval;
	}
    }
    /* init all structure members */
    *(addr->enable) = 0;
    *(addr->command) = 0;
    *(addr->feedback) = 0;
    *(addr->error) = 0;
    *(addr->output) = 0;
    addr->deadband = 0.0;
    addr->maxerror = 0.0;
    addr->maxerror_i = 0.0;
    addr->maxerror_d = 0.0;
    addr->maxcmd_d = 0.0;
    addr->error_i = 0.0;
    addr->prev_error = 0.0;
    addr->error_d = 0.0;
    addr->prev_cmd = 0.0;
    addr->limit_state = 0.0;
    addr->cmd_d = 0.0;
    addr->bias = 0.0;
    addr->pgain = 1.0;
    addr->igain = 0.0;
    addr->dgain = 0.0;
    addr->ff0gain = 0.0;
    addr->ff1gain = 0.0;
    addr->maxoutput = 0.0;
    /* export function for this loop */
    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.do-pid-calcs", num);
    retval =
	hal_export_funct(buf, calc_pid, &(pid_array[num]), 1, 0, comp_id);
    if (retval != 0) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "PID: ERROR: do_pid_calcs funct export failed\n");
	hal_exit(comp_id);
	return -1;
    }
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}