clear
set(0,'DefaultFigureWindowStyle','docked') % Figures 'docked' or 'normal'
% State space equations:
% LM*dot_X = AM*X + BM*U
%        Y = CM*X + DM*U
 
% Symbolic Parameters of the POG block scheme:
syms  V_1 I_1 I_1_0 L_a I_a R_a K_m J_m w_m b_m C_p w_8 w_8_0 s
 
LM = ...    % Energy matrix LM:
 [L_a,   0 ;...
    0, J_m];

AM = ...    % Power matrix AM:
 [-R_a, -K_m ;...
   K_m, -b_m];

BM = ...    % Input matrix BM:
 [ 1, 0 ;...
   0, 1];

CM = ...    % Output matrix CM:
 [ 1, 0 ;...
   0, 1];

DM = ...    % Input-output matrix DM:
 [ 0, 0 ;...
   0, 0];

X  = ...    % State vector X:
 [ I_a ;...
   w_m];

U  = ...    % Input vector U:
 [ V_1 ;...
   C_p];

Y  = ...    % Output vector Y:
 [ I_1 ;...
   w_8];

%%%%%%%%%%  SYMBOLIC TRANSFER MATRIX  %%%%%%%%%%%%%%%%%%%%%
Hsym=CM*inv(s*LM-AM)*BM+DM;
 
%%%%%%%%%%%%   SYSTEM UNITS  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 sec=1; ms=0.001*sec; min=60*sec; h=60*min; V=1; mV=0.001*V; A=1; mA=0.001*A; C=1; F=1; H=1; Ohm=1; Wb=1; m=1; cm=0.01*m; mm=0.001*m; km=1000*m; N=1; kN=1000*N; kg=1; gr=0.001*kg; Pa=1; rad=1; deg=(pi/180)*rad; rpm=2*pi/min*rad/sec; Nm=1; atm=101325*Pa;
 
%%%%%%%%%%%%   NUMERICAL VALUES  %%%%%%%%%%%%%%%%%%%%%%%%%%
L_a = 0.003*H;           % 2. Inductance L_a . Internal parameter. 
R_a = 1*Ohm;             % 3. Resistance R_a . Internal parameter. 
K_m = 0.5;               % 5. Parameter. Transformer/Gyrator. 
J_m = 0.01*kg*m^2;       % 6. Inertia J_m . Internal parameter. 
b_m = 0.002*N/(m/sec);   % 7. Angular Friction b_m . Internal parameter. 
 
%%%%%%%%%%   NUMERIC TRANSFER MATRIX   %%%%%%%%%%%%%%%%%%%%%
Hsym_N=vpa(eval(CM*inv(s*LM-AM)*BM+DM),4);
Gs_V1_wm_Str=latex(vpa(Hsym_N(2,1),4));
Gs_V1_I1_Str=latex(vpa(Hsym_N(1,1),4));
MA=double(eval(sym(AM)));
MB=double(eval(sym(BM)));
MC=double(eval(sym(CM)));
MD=double(eval(sym(DM)));
ML=double(eval(sym(LM)));
Sys=dss(MA,MB,MC,MD,ML);
Sys.StateName = {'I_a','w_m'};
Sys.InputName = {'V_1','C_p'};
Sys.OutputName= {'I_1','w_8'};
disp(' ')
disp('Hsys is the matrix transfer function of the system:')
Gs_V1_I1=tf(Sys(1,1));
Gs_V1_wm=tf(Sys(2,1));
disp('Poles of the system:')
eig(ML\MA)
% figure(1001); step(Hsys,0.32795), grid on



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Diagramma di Bode
figure(1); clf; hold on 
bode(Gs_V1_wm);
grid on 

% Diagramma di Nyquist
figure(2); clf; hold on 
[Gs_Re,Gs_Im,Wb]=nyquist(Gs_V1_wm);
plot(Gs_Re(:,:),Gs_Im(:,:),'b','LineWidth',1.8)
circle=exp(1i*(0:0.01:1)*2*pi);
plot(circle,'--k','LineWidth',0.8)
grid on
axis equal

% Luogo delle radici
figure(3); clf; hold on 
rlocus(Gs_V1_wm)
% [Rk,kk]=rlocus(Gs_V1_wm);
% plot(Rk,'b','LineWidth',1.8)
% grid on

% Valore Ks per ottentere un buon tempo si assestamento
s1=-166.8333 + 300*1i;
Gs1=evalfr(Gs_V1_wm,s1);
Ks=abs(-1/Gs1);    % 
% Sistema retroazionato
GsKs0= minreal(Ks*Gs_V1_wm/(1+Ks*Gs_V1_wm));

% Risposta al gradino della funzione Gs_V1_wm e della funzione GsKs
figure(4); clf; hold on; grid on 
[y,tOut] = step(Gs_V1_wm,0.3);
plot(tOut,y,'b','LineWidth',1.8)
[y,tOut] = step(GsKs0,0.3);
plot(tOut,y,'c','LineWidth',1.8)
% Errore a regime utilizzando il guadagno Ks
G0=dcgain(Gs_V1_wm);
e_p=1/(1+Ks*G0);

% Guadagno Kast da utilizzare se si desidera un errore a regime e_p=0.01:
e_p=0.01;
Kast=(1/e_p-1)/G0;

% Risposta al gradino della funzione GsKast e della funzione C(s)*GsKast
figure(5); clf; hold on; grid on 
GsKast0= minreal(Kast*Gs_V1_wm/(1+Kast*Gs_V1_wm));
[y,tOut] = step(GsKast0,0.03);
plot(tOut,y,'m','LineWidth',1.8)

% Sintesi di una rete correttrice  
s=tf('s');
Mf=60;
wa=1200;
B=exp(1i*(180+Mf)/180*pi);
A=evalfr(Kast*Gs_V1_wm,1i*wa);
M=abs(B)/abs(A);
phi=angle(B)-angle(A);
tau1=(M-cos(phi))/(wa*sin(phi));
tau2=(cos(phi)-1/M)/(wa*sin(phi));
Cs=(1+tau1*s)/(1+tau2*s);

% Risposta al gradino della funzione  Cs*GsKast
GsCs0= minreal(Kast*Cs*Gs_V1_wm/(1+Kast*Cs*Gs_V1_wm));
[y,tOut] = step(GsCs0,0.03);
plot(tOut,y,'r','LineWidth',1.8)

% Poli del sistema retroazionato senza rete sorrettrice  
pole(GsKast0)
% Poli del sistema retroazionato finale 
pole(GsCs0)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% % Sintesi di un controllore con azione integrale
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Gs_Int = Gs_V1_wm/s;
K_Int=40; 
Cs = (1+0.03258*s)/(1+0.003983*s);
Gs0_Int= minreal(Gs_Int/(1+Gs_Int));
Gs0_K_Int= minreal(K_Int*Gs_Int/(1+K_Int*Gs_Int));
Gs0_Cs_K_Int= minreal(K_Int*Cs*Gs_Int/(1+K_Int*Cs*Gs_Int));


% Diagrammi di Nyquist
figure(12); clf; hold on 
[Gs_Re,Gs_Im,Wb]=nyquist(Gs_Int);
plot(Gs_Re(:,:),Gs_Im(:,:),'b','LineWidth',1.8)
[Gs_Re,Gs_Im,Wb]=nyquist(K_Int*Gs_Int);
plot(Gs_Re(:,:),Gs_Im(:,:),'m','LineWidth',1.8)
[Gs_Re,Gs_Im,Wb]=nyquist(K_Int*Cs*Gs_Int);
plot(Gs_Re(:,:),Gs_Im(:,:),'r','LineWidth',1.8)
circle=exp(1i*(0:0.01:1)*2*pi);
plot(circle,'--k','LineWidth',0.8)
grid on
axis([-1.4 0.2 -1 0.2])
axis equal

% Luogo delle radici
figure(13); clf; hold on 
rlocus(Gs_Int)
% grid on

% Risposta al gradino della funzione  Cs*GsKast
figure(16); clf; hold on; grid on 
[y,tOut] = step(Gs0_Int,0.6);
plot(tOut,y,'b','LineWidth',1.8)
[y,tOut] = step(Gs0_K_Int,0.6);
plot(tOut,y,'m','LineWidth',1.8)
[y,tOut] = step(Gs0_Cs_K_Int,0.6);
plot(tOut,y,'r','LineWidth',1.8)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Controllo di corrente 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K_I=12532;
Gs_V1_I1_0= minreal(K_I*Gs_V1_I1/(1+K_I*Gs_V1_I1));
pole(Gs_V1_I1_0)

% Risposta al gradino della funzione  Cs*GsKast
figure(26); clf; hold on; grid on 
[y,tOut] = step(Gs_V1_I1_0);
plot(tOut,y,'b','LineWidth',1.8)

% Calcolo del nuovo sistema retroazioanto 
syms Iref
MXK_I=[-K_I 0; 0 0];
MUK_I=[K_I 0; 0 1];
AM_I=MA+MB*MXK_I;
BM_I=MB*MUK_I;
U_I=[Iref;C_p];

MA1=double(eval(sym(AM_I)));
MB1=double(eval(sym(BM_I)));
MC1=double(eval(sym(CM)));
MD1=double(eval(sym(DM)));
ML1=double(eval(sym(LM)));
Sys1=dss(MA1,MB1,MC1,MD1,ML1);
Sys1.StateName = {'I_a','w_m'};
Sys1.InputName = {'Iref','C_p'};
Sys1.OutputName= {'I_1','w_8'};

Gs_Iref_Wm=tf(Sys1(2,1));
pole(Gs_Iref_Wm)
Gs_Iref_Wm_0=dcgain(Gs_Iref_Wm);

% Diagrammi di Nyquist
figure(32); clf; hold on 
[Gs_Re,Gs_Im,Wb]=nyquist(Gs_Iref_Wm);
plot(Gs_Re(:,:),Gs_Im(:,:),'b','LineWidth',1.8)
circle=exp(1i*(0:0.01:1)*2*pi);
plot(circle,'--k','LineWidth',0.8)
grid on
% axis([-1.4 0.2 -1 0.2])
axis equal

% Guadagno proporzionale di velocità:

e_p=0.001;
K_w_m=(1/e_p-1)/Gs_Iref_Wm_0;

% Sistema retroazionato in velocità:
Gs0_Wref_Wm=minreal(K_w_m*Gs_Iref_Wm/(1+K_w_m*Gs_Iref_Wm));
Gs0_Wref_Wm_0=dcgain(Gs0_Wref_Wm);
pole(Gs0_Wref_Wm)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Controllo di posizione 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Sistema da controllare 

Gs0_Wref_Thm=minreal(Gs0_Wref_Wm/s);

% Diagrammi di Nyquist
figure(42); clf; hold on 
[Gs_Re,Gs_Im,Wb]=nyquist(100*Gs0_Wref_Thm);
plot(Gs_Re(:,:),Gs_Im(:,:),'b','LineWidth',1.8)
circle=exp(1i*(0:0.01:1)*2*pi);
plot(circle,'--k','LineWidth',0.8)
grid on
 axis([-1.4 0.2 -1 0.2])
axis equal


allmargin(100*Gs0_Wref_Thm)
