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