jens
1ab94a4bea
git-svn-id: http://moon:8086/svn/matlab/trunk@99 801c6759-fa7c-4059-a304-17956f83a07c
96 lines
2.1 KiB
Matlab
Executable File
96 lines
2.1 KiB
Matlab
Executable File
function pid_eval(Nd, kp, ki, kd)
|
|
|
|
% pid_eval(6, 0.002, 2.0e-6, 8.0)
|
|
% pid_eval(10, 0.0005, 1.0e-6, 4.0)
|
|
|
|
% Constants
|
|
kNoise = 0.05;
|
|
aNoise = 0.5;
|
|
alpha_i = 0.999
|
|
|
|
% Init
|
|
x = [zeros(1, 40), 2*ones(1, 160), zeros(1, 40)];
|
|
noise = kNoise*randn(length(x));
|
|
|
|
zfp = struct('velo', 0, 'dist', 0, 'x', 0);
|
|
zfc = []
|
|
|
|
ev = zeros(1, length(x));
|
|
efv = zeros(1, length(x));
|
|
y1v = zeros(1, length(x));
|
|
y2v = zeros(1, length(x));
|
|
ypv = zeros(1, length(x));
|
|
yiv = zeros(1, length(x));
|
|
ydv = zeros(1, length(x));
|
|
|
|
yvelo = 0;
|
|
ydist = 0;
|
|
state = 'Init';
|
|
y_last = 0;
|
|
|
|
b_f = [aNoise];
|
|
a_f = [1 -(1-aNoise)];
|
|
z_f = zeros(1, max(length(a_f), length(b_f))-1);
|
|
|
|
for n=1:length(x),
|
|
e = x(n)-ydist + noise(n);
|
|
[ef, z_f] = filter(b_f, a_f, e, z_f);
|
|
|
|
[ypid, yc, zfc] = jpid(zfc, kp, ki, kd, alpha_i, ef);
|
|
state_next = state;
|
|
if strcmp(state, 'Init')
|
|
state_next = 'Acquisition';
|
|
elseif strcmp(state, 'Acquisition')
|
|
if abs(ef) < 0.001
|
|
kp = kp * 1.0;
|
|
ki = ki * 1.0;
|
|
kd = kd * 1.0;
|
|
state_next = 'Track';
|
|
end
|
|
elseif strcmp(state, 'Track')
|
|
if abs(ef) > 0.01
|
|
kp = kp * 1.0;
|
|
ki = ki * 1.0;
|
|
kd = kd * 1.0;
|
|
state_next = 'Acquisition';
|
|
end
|
|
end
|
|
if strcmp(state_next, state) == 0
|
|
disp(state);
|
|
end
|
|
dy = ypid - y_last;
|
|
y_last = ypid;
|
|
[yvelo, ydist, zfp] = plant(zfp, Nd, dy);
|
|
ev(n) = e;
|
|
efv(n) = ef;
|
|
y1v(n) = ypid;
|
|
y2v(n) = ydist;
|
|
ypv(n) = zfc.yp;
|
|
yiv(n) = zfc.yi;
|
|
ydv(n) = zfc.yd;
|
|
state = state_next;
|
|
end
|
|
|
|
if 1
|
|
subplot(3, 1, 1)
|
|
plot(1:length(x), ev, 1:length(x), efv); grid; legend("Error", "Error_{Filtered}")
|
|
subplot(3, 1, 2)
|
|
plot(1:length(x), y1v, 1:length(x), ypv, 1:length(x), yiv, 1:length(x), ydv); grid; legend("PID", "P", "I", "D")
|
|
subplot(3, 1, 3)
|
|
plot(1:length(x), x, 1:length(x), y2v); grid; legend("Target", "Plant")
|
|
end
|
|
|
|
function [yvelo, ydist, zf] = plant(zi, nd, x)
|
|
b = [zeros(1, nd-1) 0.01];
|
|
a = [1 -1];
|
|
if zi.velo == 0
|
|
zi.velo = zeros(1, max(length(a), length(b))-1);
|
|
end
|
|
if zi.dist == 0
|
|
zi.dist = zeros(1, 1);
|
|
end
|
|
|
|
[yvelo, zf.velo] = filter(b, a, x, zi.velo);
|
|
[ydist, zf.dist] = filter([1], [1 -1], yvelo, zi.dist);
|
|
|