%   ff_rhpz.m
%   Illustration of feedforward compensation
%   kja 2012

aminit;					% initialize FBS macros

s=tf([1 0],1);
z=0.707;

wc=0.2;tmax=50;dt=tmax/100;t=0:dt:tmax; 
Gm=wc^2*(1-s)/(s^2+2*z*wc*s+wc^2);
Gm0=wc^2/(s^2+2*z*wc*s+wc^2);
Gff=wc^2*(s+1)^2/(s^2+2*z*wc*s+wc^2);
ym=step(Gm,t);
ym0=step(Gm0,t);
uff=step(Gff,t);
subplot(331)
pl=plot(t,ym,'b',t,ym0,'b--');
set(pl,  'LineWidth',  AM_data_linewidth);
ylabel('y');
amaxis([0 tmax 0 1.2]);
subplot(334)
pl=plot(t,uff,'b-');
set(pl,  'LineWidth',  AM_data_linewidth);
xlabel('t');ylabel('uff');
amaxis([0 tmax 0 1.2]);
amprint('ff-rhpz-wc02.eps');

wc=1;tmax=10;dt=tmax/100;t=0:dt:tmax; 
Gm=wc^2*(1-s)/(s^2+2*z*wc*s+wc^2);
Gm0=wc^2/(s^2+2*z*wc*s+wc^2);
Gff=wc^2*(s+1)^2/(s^2+2*z*wc*s+wc^2);
ym=step(Gm,t);
ym0=step(Gm0,t);
uff=step(Gff,t);
subplot(331)
pl=plot(t,ym,'b',t,ym0,'b--');
set(pl,  'LineWidth',  AM_data_linewidth);
ylabel('y');
amaxis([0 tmax -0.3 1.2]);
subplot(334)
pl=plot(t,uff,'b-');
set(pl,  'LineWidth',  AM_data_linewidth);
xlabel('t');ylabel('uff');
amaxis([0 tmax 0 1.5]);
amprint('ff-rhpz-wc1.eps');

wc=5;tmax=2;dt=tmax/100;t=0:dt:tmax; 
Gm=wc^2*(1-s)/(s^2+2*z*wc*s+wc^2);
Gm0=wc^2/(s^2+2*z*wc*s+wc^2);
Gff=wc^2*(s+1)^2/(s^2+2*z*wc*s+wc^2);
ym=step(Gm,t);
ym0=step(Gm0,t);
uff=step(Gff,t);
subplot(331)
pl=plot(t,ym,'b',t,ym0,'b--');
set(pl,  'LineWidth',  AM_data_linewidth);
ylabel('y');
amaxis([0 tmax -2 1.5]);
subplot(334)
pl=plot(t,uff,'b-');
set(pl,  'LineWidth',  AM_data_linewidth);
xlabel('t');ylabel('uff');
amaxis([0 tmax -5 30]);
amprint('ff-rhpz-wc5.eps');
break

    axis([0 tmax YL(kkk) YH(kkk)]);
    line([0 tmax],[0 0]);
    subplot(Wu(kkk))
    plot(t,uff);
    axis([0 tmax UL(kkk) UH(kkk)]);

subplot(331)
pl = plot(t, ym, str);
set(pl,  'LineWidth',  AM_data_linewidth);
ylabel('y');
amaxis([0 tmax 0 1.2]);
subplot(334)
pl = plot(t, uff, str);
set(pl,  'LineWidth',  AM_data_linewidth);
xlabel('t');
ylabel('uff');
amaxis([0 tmax 0 1.2]);
amprint('ff-simp-wc1.eps')
t=0:0.01:8;
Wc=[0.2 1 5];
Wy=[331 332 333];
Wu=[334 335 336];
TT=[50 10 2];
AA=[1.2 6 60];
YH=[1.3 1.3 1.3];
YL=[-0.3 -0.3 -2];
UH=[1.5 1.5 30];
UL=[0 0 -5];

for kkk=1:3;
    tmax=TT(kkk);dt=tmax/100;
    t=0:dt:tmax; 
    wc=Wc(kkk);
    Gm=wc^2*(1-s)/(s^2+2*z*wc*s+wc^2);
    Gm0=wc^2/(s^2+2*z*wc*s+wc^2);
    Gff=wc^2*(s+1)^2/(s^2+2*z*wc*s+wc^2);
    ym=step(Gm,t);
    ym0=step(Gm0,t);
    uff=step(Gff,t);
    subplot(Wy(kkk))
    plot(t,ym,'b',t,ym0,'b--');
    axis([0 tmax YL(kkk) YH(kkk)]);
    line([0 tmax],[0 0]);
    subplot(Wu(kkk))
    plot(t,uff);
    axis([0 tmax UL(kkk) UH(kkk)]);
end
%subplot(333)
%axis([0 tmax -2 1.3])
print -depsc ff-rhpz.eps