- refactored

common/system_control/jpid.m

@@ -0,0 +1,36 @@
+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));

common/system_control/pid_eval.m

@@ -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);
+