% Kap 11.3
% Fasavancerande kompensering
% Motor G(s)=100/(s(s+10))
% M?l: ?ka snabbheten faktor 2, med samma fasmarginal

s = tf('s');
gp = 100/(s*(s+10));
t = 0:0.01:1;

% Before compensation:
figure(1)
gcl = feedback(gp,1);
y = step(gcl,t); 
plot(t,y); grid on; hold on

figure(2)
wmin=1e-1;wmax=1e2;
bode(gp,{wmin,wmax}); grid on; hold on;
marg=allmargin(gp)

figure(3)
gu=feedback(1,gp);
u=step(gu,t);
plot(t,u,'b'); grid on; hold on;

% Compute phase advance link
wc = marg.PMFrequency
wcny = 2*wc;
phaseneeded=phase(evalfr(gp,i*wc)) - phase(evalfr(gp,i*wcny))
N=fzero(@(N) atan(sqrt(N))-atan(sqrt(1/N))-phaseneeded,3)
%N=1.9926;
b = wcny/sqrt(N);
Kk = 1/(sqrt(N)*abs(evalfr(gp,i*wcny)))
gk = Kk*N*(s+b)/(s+b*N)

% Simulate result - looks fine:
figure(1)
gcl=feedback(gk*gp,1);
y1=step(gcl,t);
plot(t,y1,'r'); 

figure(2)
bode(gk*gp,'r')

figure(3)
gu=feedback(gk,gp);
u=step(gu,t);
plot(t,u,'r')

pause

% What about just increasing the gain?
% Gives problems with overshoot and bad damping:

K = abs(evalfr(gp,i*wc)/evalfr(gp,i*wcny))

figure(1)
gcl2=feedback(K*gp,1);
y2=step(gcl2,t);
plot(t,y2,'k'); grid on
legend('K=1','K(s+b)/(s+bN)','K=2.93')

figure(2)
bode(K*gp,'k')
legend('K=1','K(s+b)/(s+bN)','K=2.93')

figure(3)
gu2=feedback(K,gp);
u2=step(gu2,t);
plot(t,u2,'k')

legend('K=1','K(s+b)/(s+bN)','K=2.93')

% Pole-zero location of the resulting system

figure(4)
closedloopwithlead=zpk(gcl)
rlocus(gk*gp);hold on 
R=rlocus(gk*gp,1)
plot(R,'*r')

closedloopwithK=zpk(gcl2)
rlocus(K*gp);hold on 
R=rlocus(gp,1)
plot(R,'*b')
R2=rlocus(K*gp,1)
plot(R2,'*k')
axis([-40 0 -30 30])

title('Root locus K=1(blue), phaselead (red), K=2.93 (black)')


