- refactored

2022-06-30 13:32:40 +02:00
parent 77cd5261b1
commit 776932e5d1
144 changed files with 0 additions and 38 deletions
@@ -0,0 +1,95 @@
+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);
+