% Example Problem 7-4 p 557 Ogata
% Lag Compensation
disp(' '); disp('Start here <<<<<<<<<<<<<<<<<<<<<'); disp(' ')
disp('Design specs')
disp(' Velocity error constant Kv=5 per sec')
disp(' Phase margin >40 deg')
disp(' Gain margin >10 db')
disp('Open loop system G')
G=zpk([],[0 -1 -2],2)
ifig=0
ifig=ifig+1; figure(ifig)
step(G)
title('Open-loop step response (output does not track input)')
% [EssStep,Kp,EssRamp,Kv,EssParabola,Ka,type]=ErrorConst(SISOsys)
disp('Check velocity error constant for this type 1 system')
[EssStep,Kp,EssRamp,Kv,EssParabola,Ka,type]=ErrorConst(G)
str1=' Desired Kv is 5. Actual Kv is ';
disp([str1,num2str(Kv)])
disp('Add gain of 5 to open loop and check error constant')
K=5
[EssStep,Kp,EssRamp,Kv,EssParabola,Ka,type]=ErrorConst(K*G)
ifig=ifig+1; figure(ifig)
bode(K*G)
title('Fig 7-30, Bode Diagram of G1=5*G(s)')
grid on
[GmG1,PmG1,WcgG1,WcpG1]=margin(K*G)
GmG1_dB = 20*log10(GmG1)
disp('G1=K*G')
K*G
disp('Phase margin is negative. Need to include a lag compensator with DC gain of 1 of the following form.')

disp('Gc1')
Gc1=zpk([-.1],[-.01],0.1)
ifig=ifig+1; figure(ifig)
bode(Gc1)
grid on
title('Bode plot of Gc1(s)=0.1(s+0.1)/(s+0.01)')

ifig=ifig+1; figure(ifig)
bode(Gc1*K*G)
grid on
title('Gc1*K*G')
%[Gm,Pm,Wcg,Wcp] = MARGIN(SYS)
[GmGc1KG,PmGc1KG,WcgGc1KG,WcpGc1KG]=margin(Gc1*K*G)
GmGc1KG_dB = 20*log10(GmGc1KG)
str2='Phase margin is now '; disp([str2,num2str(PmGc1KG),' deg'])
str3='Gain margin is now '; disp([str3,num2str(GmGc1KG_dB),' db'])

ifig=ifig+1; figure(ifig)
cl=feedback(Gc1*K*G,1)
step(cl)
title('Closed-loop Step response')

ifig=ifig+1; figure(ifig)
bode(cl)
grid on
title('Closed loop = Gc1*K*G/(1+Gc1*K*G)')

bw=bandwidth(cl)