function [kp,ki,kd,troll,kpid] = despid(oploop,pm_des,pm_freq,pi_freq,roll_freq)
% function [kp,ki,kd,troll,kpid] = despid(oploop,pm_des,pm_freq,pi_freq,roll_freq)
%
% Simple file to compute PID controller design
% NOTE this is all in Controls Toolbox format
%
% oploop    - Open loop transfer function
% pm_des    - Desired Phase Margin in Degrees               (default = 50)
% pm_freq   - Desired Crossover frequency in Hz             (default = 10)
% pi_freq   - Desired kick-in frequency for PI stage in Hz  (default = 0   - no PI stage)
% roll_freq - Desired rolloff frequency for PD stage in Hz  (default = inf - no rolloff)
%
% Peter M. Young, 2018

if nargin==0
    disp('usage: [kp,ki,kd,troll,kpid] = despid(oploop,pm_des,pm_freq,pi_freq,roll_freq)')
    return
end

% Set default values

def_roll_freq = inf;
def_pi_freq = 	0;
def_pm_freq = 	10;
def_pm_des = 	50;

if nargin < 5
    roll_freq = [];
end
if nargin < 4
    pi_freq = [];
end
if nargin < 3
    pm_freq = [];
end
if nargin < 2
    pm_des = [];
end

if isempty(roll_freq)
    roll_freq = def_roll_freq;
end
if isempty(pi_freq)
    pi_freq = def_pi_freq;
end
if isempty(pm_freq)
    pm_freq = def_pm_freq;
end
if isempty(pm_des)
    pm_des = def_pm_des;
end

roll_freq = abs(roll_freq);
pi_freq = abs(pi_freq);
pm_freq = abs(pm_freq);
pm_des = abs(pm_des);

% Simple error checks

if pi_freq >= pm_freq
    error('PM_FREQ must be greater than PI_FREQ')
    return
end

if pm_freq >= roll_freq
    error('ROLL_FREQ must be greater than PM_FREQ')
    return
end

% Now calculate PID controller coefficients

kit = 2*pi*pi_freq;
if kit==0
    kpi = 1;
else
    kpi = tf([1 kit],[1 0]);
end

troll = 1/(2*pi*roll_freq);
if troll==0
    kroll = 1;
else
    kroll = tf(1,[troll 1]);
end

loopcon = oploop*kpi*kroll; % Roll in PI and rolloff stages

gcomp = frdata(frd(loopcon,2*pi*pm_freq));
gphase = (180/pi)*angle(gcomp);
gmag = abs(gcomp);

thetadeg = -180+pm_des-gphase;
theta = (pi/180)*thetadeg;

kpt = cos(theta)/gmag;
kdt = sin(theta)/(gmag*2*pi*pm_freq);

kp = kpt+(kit*kdt);       % Effective PID coefficients
ki = kpt*kit;
kd = kdt;

kpid = (kp + ki*tf(1,[1 0]) + kd*tf([1 0],1))*kroll;

% Final check for closed-loop stability

loopfin = oploop*kpid;
mclos = feedback(loopfin,1);
pclos = pole(mclos);
stabflag = max(real(pclos));

if stabflag >= 0
    disp(' ');
    disp('WARNING: closed-loop system is UNSTABLE');
    disp(' ');
end



