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 */    /* save old value for 2nd derivative calc later */    tmp2 = pid->cmd_d;    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;	}    }    /* calculate 2nd derivative of command */    pid->cmd_dd = (pid->cmd_d - tmp2) * periodrecip;    /* apply 2nd derivative limits */    if (pid->maxcmd_dd != 0.0) {	if (pid->cmd_dd > pid->maxcmd_dd) {	    pid->cmd_dd = pid->maxcmd_dd;	} else if (pid->cmd_dd < -pid->maxcmd_dd) {	    pid->cmd_dd = -pid->maxcmd_dd;	}    }    /* 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 +	    pid->cmd_dd * pid->ff2gain;	/* 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_IN, &(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_IN, &(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_IN, &(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_OUT, &(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_OUT, &(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_RW, &(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_RW, &(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_RW, &(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_RW, &(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_RW, &(addr->maxcmd_d), comp_id);    if (retval != 0) {	return retval;    }    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxcmdDD", num);    retval = hal_param_float_new(buf, HAL_RW, &(addr->maxcmd_dd), comp_id);    if (retval != 0) {	return retval;    }    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.bias", num);    retval = hal_param_float_new(buf, HAL_RW, &(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_RW, &(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_RW, &(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_RW, &(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_RW, &(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_RW, &(addr->ff1gain), comp_id);    if (retval != 0) {	return retval;    }    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.FF2", num);    retval = hal_param_float_new(buf, HAL_RW, &(addr->ff2gain), comp_id);    if (retval != 0) {	return retval;    }    rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.maxoutput", num);    retval = hal_param_float_new(buf, HAL_RW, &(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_RO, &(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_RO, &(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_RO, &(addr->cmd_d), comp_id);	if (retval != 0) {	    return retval;	}	rtapi_snprintf(buf, HAL_NAME_LEN, "pid.%d.commandDD", num);	retval = hal_param_float_new(buf, HAL_RO, &(addr->cmd_dd), 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->maxcmd_dd = 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->cmd_dd = 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->ff2gain = 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;}