// SQ file for SISO discrete-time systems with PID controller. // Version 2.2.2, 24.6.2002 // Copyright 1998-2002, Calerga. // // Disclaimer // // Absolutely no guarantee is made about the content of this file, the // underlying algorithms, their suitability with respect to any particular // application, or the functionning of the code with any software. The user // supports the whole responsibility of the reading and the use of this file // and the use of the results which might be obtained with it. // // Permission is granted to licensees of SQ to use, modify, and distribute // modified versions of this file or new files which borrow some code from // it to other licensees of SQ, provided that its original author (Yves Piguet) // is acknowledged and that it is made clear that the new file is a derivated // work. The licensee assumes all the legal consequences which might result // from the use and the distribution of the derivative work. // // The "system" block at the end of this file, if there is one, was added // after the release of this file and is even more outside of the scope of // the author's responsibility. // We define the digital PID controller as // K(z) = kp + ki / (z - 1) + kd (z - 1) / z // or // K(z) = Kp (1 + 1 / (Ti (z - 1)) + Td (z - 1) / z) // with Kp = kp, Ti = kp / ki, Td = kd / kp, ki = Kp / Ti, and kd = Kp * Td // Special cases: // P: K(z) = kp // PI: K(z) = (kp z - kp + ki) / (z - 1) // PD: K(z) = ((kp + kd) z - kd) / z // PID: K(z) = ((kp + kd) z^2 + (ki - kp - 2 kd) z + kd) / (z^2 - z) help {@ The PID controller is fundamentally a continuous-time controller. However, it is often implemented with discrete-time electronic devices (such as microcomputers, microcontrollers or fpga's). Sampling effects may change performance in subtle ways, especially when the sampling frequency is not very high with respect to the bandwidth of the controlled system. Instead of converting a continuous-time PID controller, it is possible to design a PID directly in the discrete-time domain, approximating the integration and derivation by sums and differences, respectively. The parameters of the PID keep their standard meaning. PID_dt.sq does for discrete-time PID controllers what PID_ct.sq does for continuous-time PID controllers. Taking as input the difference between the desired set-point and the measured system output ("error" e(k)), the PID controller has three terms with easy-to-understand effects which are added up, and three parameters to adjust their weights: - a proportional term (the larger the error, the larger the control signal to reduce it); - an integral term (if a nonzero control signal is required to cancel out the error, the control signal is increased until the error vanishes); - a derivative term (the evolution of the error is anticipated to increase damping). Weights can be specified either separately for the three terms (weights kp, ki, and kd respectively), or as a global gain kp and two time values Ti and Td which do not depend on the gain of the system. PID_dt.sq uses both parameterization. The control signal u(k) is u(k) = kp.e(k) + ki.Ts.sum(e(k)) + (kd/Ts).(e(k)-e(k-1)) or u(k) = kp (e(k) + sum(e(k))/(Ti/Ts) + (Td/Ts).(e(k)-e(k-1))) where kp is the global gain, Ti a time value for tuning the integral term, Td a time value for tuning the derivative term, and Ts the sampling time. The transfer function K(z) of the PID is K(z) = kp (1 + Ts/Ti(z-1) + Td(z-1)/Ts.z) For a smoother response when the set-point changes rapidly, derivation is usually applied only on the feedback term, and proportional gain is reduced on the feedforward term with a weight b<1. To reduce noise amplification at higher frequencies, the derivative term is filtered with a first-order low-pass filter with a time constant Td/N. All these features can be set in the Settings menu or in the coefficient figures. Symbols: kp, ki, kd: gains of the proportional, integral, and derivative gains; Ti, Td: parameters of the integral and derivative actions expressed as time constants; b: set-point weight; N: derivative action filtering; u(k): system input; y(k): system output; w(k): system input disturbance; r(k): set point. @} variable Ac,Bc // continuous-time model(s) ([] if lacking) variable A,B // discrete-time model(s) variable Ts // sampling time variable kp // proportional gain variable ki // integral gain variable kd // derivative gain variable R,S,T // controller expressed such that R u = T r - S y variable twoDOFs // true if the derivative gain is null for the feedforward variable b // set-point weight (used only if twoDOFs) variable N // derivator filter variable dampSpec // damping specification: [absDamp,zeta], 0 or [] if none variable dispFreq // non-[] if frequency is displayed in frequency and zero/pole plots variable w // frequency under the mouse cursor, or [] if none init (A,B,Ts,kp,ki,kd,twoDOFs,b,N) = init make (R,S,T) = calcControllerTF(kp,ki,kd,N,twoDOFs,b) import "Model" _enable(strcmp(_xdatatype,'tf s')) (B,A,Bc,Ac) = importModelTFC(Ts,_xdata) import "" _enable(strcmp(_xdatatype,'tf z')) (B,A,Ts,Bc,Ac) = importModelTFD(_xdata) export "Continuous-Time Model" _enable(~isempty(Ac)) (_xdata,_xdatatype) = exportModelTFC(Bc,Ac) export "Discrete-Time Model" (_xdata,_xdatatype) = exportModelTFD(B,A,Ts) export "Controller" (_xdata,_xdatatype) = exportController(R,S,T) menu "System (continuous-time model)..." (B, A, Bc, Ac) = systemCTDialog(Bc, Ac, Ts) menu "System (discrete-time model)..." (B, A, Bc, Ac) = systemDialog(B, A) menu "Sampling Time..." (Ts, B, A) = samplingTime(Ts, Bc, Ac, B, A) separator menu "PID Coefficients..." (kp, ki, kd) = controllerDialog(kp, ki, kd) menu "Two Degrees Of Freedom" _checkmark(twoDOFs) twoDOFs = not(twoDOFs) menu "Set-Point Weight..." _enable(twoDOFs) b = setpointWeight(b) menu "Derivative Gain Limitation..." _enable(kd~=0) N = derivativeLim(N) separator menu "Damping Specifications..." _checkmark(~isempty(dampSpec)) dampSpec = dampSpecDialog(dampSpec) separator menu "Display Frequency Line" _checkmark(~isempty(dispFreq)) dispFreq = toggleDispFreq(dispFreq) figure "Step Response Y/U" draw drawOLStep(A,B,Ts) mouseover _msg = overT(_x) figure "Impulse Response Y/U" draw drawOLImpulse(A,B,Ts) mouseover _msg = overT(_x) separator figure "PID Coefficients" draw drawPIDCoeff(kp,ki,kd,twoDOFs,b,N) mousedrag (kp,ki,kd,twoDOFs,b,N) = dragPIDCoeff(kp,ki,kd,twoDOFs,b,N,_id,_nb,_x1) figure "PID Coefficients kp-Ti-Td" draw drawPIDCoeffT(kp,ki,kd,twoDOFs,b,N) mousedrag (kp,ki,kd,twoDOFs,b,N) = dragPIDCoeffT(kp,ki,kd,twoDOFs,b,N,_id,_nb,_x1) separator figure "Step Response Y/R" draw drawStepRY(A,B,Ts,R,S,T) mouseover _msg = overT(_x) figure "Step Response U/R" draw drawStepRU(A,B,Ts,R,S,T) mouseover _msg = overT(_x) figure "Step Response Y/W" draw drawStepWY(A,B,Ts,R,S) mouseover _msg = overT(_x) figure "Step Response Y/D" draw drawStepDY(A,B,Ts,R,S) mouseover _msg = overT(_x) figure "Step Response U/D" draw drawStepDU(A,B,Ts,R,S) mouseover _msg = overT(_x) figure "Ramp Response Y/R" draw drawRampRY(A,B,Ts,R,S,T) mouseover _msg = overT(_x) figure "Ramp Response Y/D" draw drawRampDY(A,B,Ts,R,S) mouseover _msg = overT(_x) separator figure "Continuous-Time Step Resp. Y/R" draw drawCTStepRY(A,B,R,S,T,Ts,Ac,Bc) mouseover _msg = overT(_x) figure "Continuous-Time Step Resp. Y/D" draw drawCTStepDY(A,B,R,S,Ts,Ac,Bc) mouseover _msg = overT(_x) separator figure "Bode Magnitude" draw drawBodeMag(A,B,Ts,R,S,w) mousedrag (kp,ki,kd,w) = dragControllerMag(kp,ki,kd,dispFreq,_id,_x1,_kx,_ky) mouseover (_msg, _cursor, w) = overWBodeAmpl(dispFreq, _x, _id) mouseout w = emptyW figure "Bode Phase" draw drawBodePhase(A,B,Ts,R,S,w) mouseover (_msg, w) = overW(dispFreq, _x) mouseout w = emptyW figure "Nyquist" draw drawNyquist(A,B,R,S,Ts,w) figure "Nichols" draw drawNichols(A,B,R,S) mousedrag (kp,ki,kd) = dragNichols(kp,ki,kd,_ky) mouseover _cursor = overObject(_id) separator figure "Sensitivity" draw drawSensMag(A,B,Ts,R,S,w) mouseover (_msg, w) = overW(dispFreq, _x) mouseout w = emptyW figure "Complementary Sensitivity" draw drawSensCMag(A,B,Ts,R,S,w) mouseover (_msg, w) = overW(dispFreq, _x) mouseout w = emptyW figure "Perturbation-Input Sensitivity" draw drawSensDUMag(A,B,Ts,R,S,w) mouseover (_msg, w) = overW(dispFreq, _x) mouseout w = emptyW separator figure "Root Locus" draw drawRLocus(A, B, R, S, dampSpec, w, Ts) mousedrag (kp, ki, kd, _msg) = dragRLocus(A, B, kp, ki, kd, N, _id, _z0, _z1, _q) mouseover (_msg, _cursor) = overRLocus(_id, _z0) separator figure "Robustness Margins" draw dispMargins(A,B,Ts,R,S) function {@ function (A,B,Ts,kp,ki,kd,twoDOFs,b,N) = init A = [1, -0.95]; B = 0.0975; Ts = 1; kp = 4; ki = 1; kd = 0.2; b = 1; N = 10; twoDOFs = false; subplots(['PID Coefficients\tStep Response Y/R\tBode Magnitude\n',... 'Nyquist\tRoot Locus\tBode Phase']); function (B,A,Bc,Ac) = importModelTFC(Ts,xdata) Bc = xdata.num; Ac = xdata.den; (B, A) = c2dm(Bc, Ac, Ts, 'z'); function (B,A,Ts,Bc,Ac) = importModelTFD(xdata) B = xdata.num; A = xdata.den; Ts = xdata.Ts; Bc = []; Ac = []; function (xdata,xdatatype) = exportModelTFC(Bc,Ac) xdata.num = Bc; xdata.den = Ac; xdatatype = 'tf s'; function (xdata,xdatatype) = exportModelTFD(B,A,Ts) xdata.num = B; xdata.den = A; xdata.Ts = Ts; xdatatype = 'tf z'; function (xdata,xdatatype) = exportController(R,S,T) xdata.R = R; xdata.S = S; if T ~== S xdata.T = T; end xdatatype = 'rst d'; function (B,A,Bc,Ac) = systemDialog(B,A) (B,A) = dialog('Coefficients of continuous-time system numerator and denominator:', ... B, A); if size(A, 1) ~= size(B, 1) dialog('A and B must have the same number of lines.'); cancel; end if size(A, 2) <= size(B, 2) dialog('B/A must be strictly causal (A must have more columns than B).'); cancel; end Bc = []; Ac = []; function (B,A,Bc,Ac) = systemCTDialog(Bc,Ac,Ts) (Bc, Ac) = dialog(['Coefficients of continuous-time system numerator ',... 'and denominator (empty = none):'], Bc, Ac); if size(Ac, 1) ~= size(Bc, 1) dialog('A and B must have the same number of lines.'); cancel; end if size(Ac, 2) <= size(Bc, 2) dialog('B/A must be strictly causal (A must have more columns than B).'); cancel; end (B, A) = c2dm(Bc, Ac, Ts, 'z'); function (kp,ki,kd) = controllerDialog(kp0,ki0,kd0) if ki0 ~= 0 Ti = kp0 / ki0; else Ti = []; end Td = kd0 / kp0; (kp, Ti, Td) = dialog('Coefficients kp,Ti,Td of the PID ([] for inexistant term):', kp0, Ti, Td); if length(Ti) == 1 ki = kp / Ti; else ki = 0; end if length(Td) == 1 kd = kp * Td; else kd = 0; end function b = setpointWeight(b) b = dialog('Set-point relative weight (typically between 0 and 1):', b); function N = derivativeLim(N) N = dialog('Derivative Gain Limitation N (typically 5-10):', N); function dampSpec = dampSpecDialog(dampSpec) if isempty(dampSpec) dampSpec = [0, 0]; end while true dampSpec = dialog('Damping specifications ([absolute,zeta]):', dampSpec); dampSpec = dampSpec(:)'; if size(dampSpec) ~== [1, 2] && dampSpec ~== [] if ~dialog('Please enter the specification as a vector of two elements.') cancel; end else break; end end if dampSpec === [0, 0] dampSpec = []; end function (Ts, B, A) = samplingTime(Ts, Bc, Ac, B, A) Ts = dialog('Sampling Time:', Ts); if isempty(Ts) | Ts <= 0 Ts = 1; end if ~isempty(Ac) && ~isempty(Bc) % convert Bc/Ac using a zero-order hold (B, A) = c2dm(Bc, Ac, Ts, 'z'); end function dispFreq = toggleDispFreq(dispFreq) dispFreq = dispFreq ? [] : true; function drawRLocus(A, B, R, S, dampSpec, w, Ts) scale('equal', [-1.1,1.1,-1.1,1.1]); zgrid; if ~isempty(dampSpec) if dampSpec(1) ~= 0 circle(0, 0, dampSpec(1), 'r'); end if dampSpec(2) ~= 0 zgrid(dampSpec(2), [], 'r'); end end rlocus(conv(B,S,2), conv(A,R,2), '', 1); plotroots(addpol(conv(A,R,2), conv(B,S,2)), '^', 1); plotroots(A, 'x', 100); plotroots(B, 'o', 101); plotroots(R, 'xb', 2); plotroots(S, 'or',3); if ~isempty(w) zgrid([], w*Ts, 'm'); end function (kp,ki,kd,msg) = dragRLocus(A,B,kp,ki,kd,N,id,z0,z1,q) if id ~== 1 && id ~== 3 cancel; end switch id case 1 % pole kp = q * kp; ki = q * ki; kd = q * kd; msg = sprintf('Gain multiplied by %g', q); case 3 % controller zero(s) (R, S) = calcControllerTF(kp, ki, kd, N); (S, zz) = movezero(S, z0, z1); if kd == 0 % PI: S = [kp, ki-kp] kp = S(1); ki = S(2) + kp; elseif ki == 0 % PD: S = [kp+kd, -kd] kp = sum(S); kd = -S(2); else % PID: S = [kp+kd, ki-kp-2*kd, kd] kd = S(3); kp = S(1) - kd; ki = S(2) + kp + 2 * kd; end msg = overRLocus(id, zz); end function msg = dispComplex(s, z) if imag(z) == 0 msg = sprintf('%s%.2g', s, z); elseif imag(z) > 0 msg = sprintf('%s%.2g+%.2gj', s, real(z), imag(z)); else msg = sprintf('%s%.2g%.2gj', s, real(z), imag(z)); end function (msg, cursor) = overRLocus(id, z) if isempty(id) cancel; end cursor = true; switch id case 1 % pole msg = dispComplex('Closed-loop pole: ', z); case 2 % R msg = dispComplex('Controller pole: ', z); case 3 % S msg = dispComplex('Controller zero: ', z); case 100 % system pole msg = dispComplex('System pole: ', z); cursor = false; case 101 % system zero msg = dispComplex('System zero: ', z); cursor = false; end function drawOLStep(A, B, Ts) dstep(B, A, Ts); function drawOLImpulse(A, B, Ts) dimpulse(B, A, Ts); function (R,S,T) = calcControllerTF(kp,ki,kd,N=1000,twoDOFs=false,b=1) % calc. the feedback S/R and feedforward T/R corresponding to a PID if kd == 0 if ki == 0 R = 1; S = kp; T = kp * b; % P else R = [1, -1]; % PI S = [kp, ki-kp]; T = [kp*b, ki-kp]; end else if ki == 0 % PD R = [1, 0]; S = [kp+kd, -kd]; T = b * kp * R; else c = 1 / (1 + N * kp / kd); R = [1, -1-c, c]; S = [kp+N*c, ki-kp*(1+c)-2*N*c, (kp-ki+N)*c]; T = [b*kp, ki-b*kp*(1+c), (b*kp-ki)*c]; end end if ~twoDOFs T = S; end function drawStepRY(A,B,Ts,R,S,T) tmax = 10 * Ts * sum(1./(0.25+abs(log(roots(A(1,:)))))); if tmax / Ts > 500 tmax = 500 * Ts; end scale([0,tmax]); num = conv(B,T,2); den = addpol(conv(B,S,2), conv(A,R,2)); dstep(1, 1, Ts, 'sc'); dstep(num, den, Ts); function drawRampRY(A,B,Ts,R,S,T) tmax = 10 * Ts * sum(1./(0.25+abs(log(roots(A(1,:)))))); if tmax / Ts > 500 tmax = 500 * Ts; end scale([0,tmax]); num = conv(B,T,2); den = conv(addpol(conv(B,S,2), conv(A,R,2)), [1, -1], 2); dstep(1, [1, -1], Ts, 'c'); dstep(num, den, Ts); function drawStepRU(A,B,Ts,R,S,T) tmax = 10 * Ts * sum(1./(0.25+abs(log(roots(A(1,:)))))); if tmax / Ts > 500 tmax = 500 * Ts; end scale([0,tmax]); num = conv(A,T,2); den = addpol(conv(B,S,2), conv(A,R,2)); dstep(num, den, Ts, 's'); function drawStepWY(A,B,Ts,R,S) tmax = 10 * Ts * sum(1./(0.25+abs(log(roots(A(1,:)))))); if tmax / Ts > 500 tmax = 500 * Ts; end scale([0,tmax]); num = conv(B,R,2); den = addpol(conv(B,S,2), conv(A,R,2)); dstep(num, den, Ts); function drawStepDY(A,B,Ts,R,S) tmax = 10 * Ts * sum(1./(0.25+abs(log(roots(A(1,:)))))); if tmax / Ts > 500 tmax = 500 * Ts; end scale([0,tmax]); num = conv(A,R,2); den = addpol(conv(B,S,2), conv(A,R,2)); dstep(num, den, Ts); function drawRampDY(A,B,Ts,R,S) tmax = 10 * Ts * sum(1./(0.25+abs(log(roots(A(1,:)))))); if tmax / Ts > 500 tmax = 500 * Ts; end scale([0,tmax]); num = conv(A,R,2); den = conv(addpol(conv(B,S,2), conv(A,R,2)), [1, -1], 2); dstep(num, den, Ts); function drawStepDU(A,B,Ts,R,S) tmax = 10 * Ts * sum(1./(0.25+abs(log(roots(A(1,:)))))); if tmax / Ts > 500 tmax = 500 * Ts; end scale([0,tmax]); num = -conv(A,S,2); den = addpol(conv(B,S,2), conv(A,R,2)); dstep(num, den, Ts, 's'); function drawCTStepRY(A,B,R,S,T,Ts,Ac,Bc) if isempty(Ac) text('No continuous-time model.'); else % numd/stepd is the transfer function between R and U numD = conv(A,T,2); denD = addpol(conv(B,S,2), conv(A,R,2)); tmax = 10 * Ts * sum(1./(0.25+abs(log(roots(A(1,:)))))); if tmax / Ts > 500 tmax = 500 * Ts; end scale([0,tmax]); dstep(1, 1, Ts, 'cs'); hstep(numD, denD, Ts, Bc, Ac); end function drawCTStepDY(A,B,R,S,Ts,Ac,Bc) if isempty(Ac) text('No continuous-time model.'); else % numd/stepd is the transfer function between D and U numD = -conv(A,S,2); denD = addpol(conv(B,S,2), conv(A,R,2)); tmax = 10 * Ts * sum(1./(0.25+abs(log(roots(A(1,:)))))); if tmax / Ts > 500 tmax = 500 * Ts; end scale([0,tmax]); hstep(numD, denD, Ts, Bc, Ac); end function (b, a) = substRat(b0, a0, ns, ds) % calc. b(w)/a(w) = b0(v)/a0(v) with v = ns(w)/ds(w), % where b, a, b0, a0, ns, and ds are polynomials n = length(a0); b0 = [zeros(1, n-length(b0)), b0]; % pad b0 with zeros b = b0(1); a = a0(1); k = 1; for i = 2:n k = conv(k, ds); b = addpol(conv(b, ns), b0(i)*k); a = addpol(conv(a, ns), a0(i)*k); end while b(1) == 0 % remove leading zeros b = b(2:end); end function drawBodeMag(A, B, Ts, R, S, w) scale('logdb',[0,pi/Ts]); line([0,1],1,'c'); dbodemag(B, A, Ts, 'b', 99); dbodemag(conv(B,S,2), conv(A,R,2), Ts, '', 99); dbodemag(S, R, Ts, 'r', 1); if size(A, 1) == 1 (gm,psi,wc,wx) = dmargin(conv(B,S,2), conv(A,R,2), Ts); if ~isempty(wc) line(repmat([1,0], length(wc), 1), wc(:), 'c'); end if ~isempty(wx) line(repmat([1,0], length(wx), 1), wx(:), 'c'); end end if ~isempty(w) line([1,0], w, 'm'); end function (kp, ki, kd, w) = dragControllerMag(kp, ki, kd, dispFreq, id, x1, kx, ky) if id ~== 1 cancel; end kp = kp * ky; ki = ki * ky * kx; kd = kd * ky / kx; if dispFreq && x1 >= 0 w = x1; else w = []; end function (msg, cursor, w) = overWBodeAmpl(dispFreq, x, id) (msg, w) = overW(dispFreq, x); cursor = id === 1; function (kp, ki, kd) = dragNichols(kp, ki, kd, ky) if isempty(ky) cancel; end kp = kp * ky; ki = ki * ky; kd = kd * ky; function drawBodePhase(A,B,Ts,R,S,w) scale('loglin'); line([0,1],-pi,'c'); dbodephase(B, A, Ts, 'b', 1); dbodephase(conv(B,S,2), conv(A,R,2), Ts); dbodephase(S, R, Ts, 'r', 1); if size(A, 1) == 1 (gm,psi,wc,wx) = dmargin(conv(B,S,2), conv(A,R,2), Ts); if ~isempty(wc) line(repmat([1,0], length(wc), 1), wc(:), 'c'); end if ~isempty(wx) line(repmat([1,0], length(wx), 1), wx(:), 'c'); end end if ~isempty(w) line([1,0], w, 'm'); end function drawNyquist(A,B,R,S,Ts,w) scale('equal',[-1.5,1.5,-1.5,1.5]); hgrid; dnyquist(conv(B,S,2), conv(A,R,2)); if ~isempty(w) z = polyval(conv(B,S,2),exp(1j*w*Ts)) ./ polyval(conv(A,R,2),exp(1j*w*Ts)); plot(real(z),imag(z),'om'); end function drawNichols(A,B,R,S) scale('lindb'); ngrid; dnichols(conv(B,S,2), conv(A,R,2), '', 1); function cursor = overObject(id) cursor = ~isempty(id) && id ~== 99; function drawPIDCoeff(kp,ki,kd,twoDOFs,b,N) msg = sprintf('kp: %.1g', kp); val = kp; lim = kp*[0.25,4]; kind = 'L'; if ki ~= 0 msg = [msg, sprintf('\nki: %.1g', ki)]; val = [val; ki]; lim = [lim; ki*[0.25,4]]; kind = [kind, 'L']; end if kd ~= 0 msg = [msg, sprintf('\nkd: %.1g', kd)]; val = [val; kd]; lim = [lim; kd*[0.25,4]]; kind = [kind, 'L']; end msg = [msg, twoDOFs?sprintf('\nb: %.1g', b):'\nb: -']; val = [val; twoDOFs?b:inf]; lim = [lim; min(0, b), max(1, b)]; kind = [kind, '-']; if kd ~= 0 msg = [msg, sprintf('\nN: %.1g', N)]; val = [val; N]; lim = [lim; 1, max(20,2*N)]; kind = [kind, '-']; end settabs('kp: 9999.9 \t\b off \t'); button('2-dof:\toff\ton', 1 + twoDOFs, 'radio', '', 2); slider(msg, val, lim, kind, '', 1); function (kp,ki,kd,twoDOFs,b,N) = dragPIDCoeff(kp,ki,kd,twoDOFs,b,N,id,nb,x1) if isempty(id) cancel; end if id == 2 twoDOFs = x1 == 2; return; end what = 'p'; if ki ~= 0 what = [what, 'i']; end if kd ~= 0 what = [what, 'd']; end what = [what, 'b']; if kd ~= 0 what = [what, 'N']; end switch what(nb) case 'p' kp = x1; case 'i' ki = x1; case 'd' kd = x1; case 'b' b = x1; twoDOFs = true; case 'N' N = x1; end function drawPIDCoeffT(kp,ki,kd,twoDOFs,b,N) msg = sprintf('Kp: %.1g', kp); val = kp; lim = kp*[0.25,4]; kind = 'L'; if ki ~= 0 msg = [msg, sprintf('\nTi: %.1g', kp/ki)]; val = [val; kp/ki]; lim = [lim; kp/ki*[0.25,4]]; kind = [kind, 'L']; end if kd ~= 0 msg = [msg, sprintf('\nTd: %.1g', kd/kp)]; val = [val; kd/kp]; lim = [lim; kd/kp*[0.25,4]]; kind = [kind, 'L']; end msg = [msg, twoDOFs?sprintf('\nb: %.1g', b):'\nb: -']; val = [val; twoDOFs?b:inf]; lim = [lim; min(0, b), max(1, b)]; kind = [kind, '-']; if kd ~= 0 msg = [msg, sprintf('\nN: %.1g', N)]; val = [val; N]; lim = [lim; 1, max(20,2*N)]; kind = [kind, '-']; end settabs('Kp: 9999.9 \t\b off \t'); button('2-dof:\toff\ton', 1 + twoDOFs, 'radio', '', 2); slider(msg, val, lim, kind, '', 1); function (kp,ki,kd,twoDOFs,b,N) = dragPIDCoeffT(kp,ki,kd,twoDOFs,b,N,id,nb,x1) if isempty(id) cancel; end if id == 2 twoDOFs = x1 == 2; return; end what = 'p'; if ki ~= 0 what = [what, 'i']; end if kd ~= 0 what = [what, 'd']; end what = [what, 'b']; if kd ~= 0 what = [what, 'N']; end switch what(nb) case 'p' ki = ki * x1 / kp; kd = kd * x1 / kp; kp = x1; case 'i' ki = kp/x1; case 'd' kd = kp*x1; case 'b' b = x1; twoDOFs = true; case 'N' N = x1; end function drawSensMag(A,B,Ts,R,S,w) num = conv(A,R,2); den = addpol(num, conv(B,S,2)); scale('logdb'); dbodemag(num, den, Ts); if ~isempty(w) line([1,0],w,'m'); end function drawSensCMag(A,B,Ts,R,S,w) num = conv(B,S,2); den = addpol(num, conv(A,R,2)); scale('logdb'); dbodemag(num, den, Ts); if ~isempty(w) line([1,0],w,'m'); end function drawSensDUMag(A,B,Ts,R,S,w) scale('logdb'); num = -conv(A,S,2); den = addpol(conv(A,R,2),conv(B,S,2)); scale('logdb'); dbodemag(num, den, Ts); if ~isempty(w) line([1,0],w,'m'); end function dispMargins(A,B,Ts,R,S) gms = 'Gain margin [dB]:'; psis = 'Phase margin [deg]:'; wcs = 'Critical freq. [rad/s]:'; wxs = 'Cross-over freq. [rad/s]:'; for i = 1:size(B,1) (gm,psi,wc,wx) = dmargin(conv(B(i,:),S), conv(A(i,:),R), Ts); if isempty(gm) % unstable gms = [gms,'\t-']; wcs = [wcs,'\t-']; else (mx,ix) = max(gm); % find the positive gain margin gms = sprintf('%s\t%.2f',gms,20*log10(gm(ix))); wcs = sprintf('%s\t%.2f',wcs,wc(ix)); end if isempty(psi) % unstable or no intersection between Nyquist and the unit circle psis = [psis,'\t-']; wxs = [wxs,'\t-']; else psis = sprintf('%s\t%.2f',psis,180*psi/pi); wxs = sprintf('%s\t%.2f',wxs,wx); end end settabs('Cross-over freq. [rad/s]: \t'); text([gms,'\n',wcs,'\n',psis,'\n',wxs]); function w = emptyW w = []; function (msg, w) = overW(dispFreq, x) msg = sprintf('w = %g', x); if dispFreq && x >= 0 w = x; else w = []; end function msg = overT(x) msg = sprintf('t = %g', x); @}