% setup
clear all
close all
clc

% uncompensated dynamics
s=tf('s');          % Laplace variable
Go=2/(5*s+1);       % uncompensated open-loop

% computes all margins and corresponding frequencies
M=allmargin(Go);
wc_old=M.PMFrequency % wc of uncompensated system
pM_old=M.PhaseMargin

% design specifications
wc=2*wc_old         % cross-over frequency
pM=pM_old+15 % phase margin (in degrees)

% phase margin increase
DpM=pM-(180+angle(evalfr(Go,i*wc))/pi*180);

% solve equation for N numerically
% (corresponds to using graph in collection of formulae)
N=fminsearch(@(N)(abs(atan(sqrt(N))-atan(1/sqrt(N))-DpM/180*pi)),3);
% N=3; % value from collection of formulae

figure(1)
margin(Go)

% properties of uncompensated system
wc_old=sqrt(3)/5;   % computed by pen and paper
wc=2*wc_old;        % desired cross-over frequency

% place phase peak at wc
b=wc/sqrt(N);

% make wc actual cross-over
KK=1/sqrt(N)/abs(evalfr(Go,i*wc)); % make wc cross-over

% compensation
Gk=KK*(1+s/b)/(1+s/(b*N));
%Gk=(1+5/2*s)/(1+5/6*s); % approximation
Gok=Go*Gk;

figure(2)
margin(Gok)

figure(3)
bode(Go,'b',Gok,'r')
legend('Go','Gk*Go')




