%PI controller design using frequency response data
%examples
%tuning 
b1=1;
a2=1;
a1=1;
num=b1;
d=3;
den=conv([6 1],[8 1]);
%inner loop design proportional control
Kc1=0.6;
%relay experiment
h=0.05;
Tsim=500;
Ra=1.75;  %relay amplitude
epsilon=0.2;  % relay with hysteresis
Nsim=Tsim/h;
sim('Tuner4Inner')
figure(1)
subplot(211)
plot(t,u)
ylabel('u')
subplot(212)
plot(t,y)

ylabel('y')
xlabel('Time (sec)')
%Estimation of frequency response in closed-loop
[thetaCurrent,n,N]=FSFSingle(u,-e);
%Estimation of inner-loop frequency response
j=sqrt(-1);
w0=2*pi/(N*h);
TjwE=thetaCurrent(3);  %estimated closed-loop frequency response
Njw_in=TjwE/(1-TjwE);       %inner-loop Nyquist response
Gjw0=Njw_in/Kc1;  % estimated frequency response of inner plant
bhat=abs(Gjw0)*w0;  
dest=atan(-imag(j*Gjw0)/real(j*Gjw0))/w0;
dest=abs(dest);
%approximate model is delay with integrator
beta=1.5;
[Kc1_new,tauI1_new,tauD1_new]=rule4PIID(bhat,dest,beta);
w=logspace(-3,2,1000);
j=sqrt(-1);
G1=freqs(num,den,w);
ds=cos(d*w)-j*sin(d*w);
G1=G1.*ds;
Ny1=G1*Kc1;
figure(2)
plot(real(Ny1),imag(Ny1))
hold on
% Demonstrate the accuracy of the estimation
%grid on
plot(Njw_in,'bo')
xlabel('Real')
ylabel('Imag')

%%%%%%%
%validation of cascade control system using Nyquist plot
C1jw_new=freqs(Kc1_new*[1 1/tauI1_new],[1 0],w);
Ny1_new=G1.*C1jw_new;
%Nyquist of the PI control system
figure(3)
plot(real(Ny1_new), imag(Ny1_new))
%grid on
axis([-3 3 -3 3])
grid on
xlabel('Real')
ylabel('Imag')

Tsim=200;
%sim('Sim4Inner')
sim('Sim4InnerPID')
%sim('Sim4InnerPD')
figure(4)
plot(t,u)
hold on
ylabel('u')
xlabel('Time (sec)')
figure(8)
plot(t,y)
hold on
ylabel('y')
xlabel('Time (sec)')


