matlab/common/jpid.m

function [y, yc, z] = jpid(zi, kp, ki, kd, alpha_i, e)

if isempty(zi)
	% Integrator
	b_i = [1];
	a_i = [1 -alpha_i];
	% Differentiator
	b_d = [1 -1];
	a_d = [1];
	i = zeros(1, max(length(a_i), length(b_i))-1);
	d = zeros(1, max(length(a_d), length(b_d))-1);
	
	z = struct('b_i', b_i, 'a_i', a_i, 'b_d', b_d, 'a_d', a_d, 'i', i, 'd', d, 'y_min', 0.0, 'y_max', 1.0, 'diff_aw', 0.0);
else
	z = zi;
end 

[yi, z.i] = filter(z.b_i, z.a_i, e + z.diff_aw, z.i);
[yd, z.d] = filter(z.b_d, z.a_d, e, z.d);

z.yp = kp*e;
z.yi = ki*yi;
z.yd = kd*yd;

y = z.yp + z.yi + z.yd;

yaw = yi;
if (yaw < z.y_min)
	z.diff_aw = abs(yaw - z.y_min);
end

if (yaw > z.y_max)
	z.diff_aw = -abs(yaw - z.y_max);
end

yc = max(z.y_min, min(z.y_max, y));