36 lines
726 B
Matlab
36 lines
726 B
Matlab
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)); |