clear all
close all
clc

%%%% Parametri %%%%

kpsi=180/pi;
kphi=180/pi;
taug=3.83e-2;
a1=0.186;
b1=-0.0445;
a2=0.033;
b2=0.0294;
Bpsi=5e-3;
I=4.37e-3;
Bphi=8.69e-3;
Iphi=4.14e-3;
T1=0.1;
T2=0.25;
Kr=-0.00891;
Tor=2.7;
Tpr=0.75;
Kgyro=0.015;

%%%% Punto di lavoro %%%%

Y1=80; %% in gradi
Y2=0;  %% in gradi
U1=0.5856; %% in MU
U2=0;  %% in MU

%%%% f.d.t. del sistema %%%

num11=(2*a1*U1+b1)*kpsi;
den11=conv(conv([T1 1],[T1 1]),[I Bpsi taug*cos(Y1*pi/180)]); 
G_1_1=tf(num11,den11);

G_1_2=0;

num21=kphi*conv(Kr,[Tor 1]);
den21=conv([1 0],conv([Tpr 1],[Iphi Bphi]));
G_2_1=tf(num21,den21);

num22=kphi*b2;
den22=conv([1 0],conv([Iphi Bphi],conv([T2 1],[T2 1])));
G_2_2=tf(num22,den22);

G=[G_1_1 G_1_2;G_2_1 G_2_2];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%% Sintesi Controllore H infinito %%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Sys=ss(G); %%% sistema in spazio di stato

p=0.01; %%% parametro per lo shift 

Sys.a=Sys.a-p*eye(size(Sys.a)); %%% spostamento del polo dall'origine

%%% Definizione dei pesi %%% 
W1=[0 0.1 10;10 2 0.1;0 0.1 10;10 2 0.1];       %% su S
W2=[1;1;1;1];                                   %% sul Comando 
W3=[0 1 0 0;0 0 0 1000; 10 1 0 0; 0 0 0 1000];  %% su T

SS_g=mksys(Sys.a,Sys.b,Sys.c,Sys.d); %%% matrici del sistema inserite in una variabile 'tree'
Tss=augtf(SS_g,W1,W2,W3); %%% realizzazione in spazio di stato aumentata
[SS_CP,SS_Cf,hinfo]=hinf(Tss); %%% calcolo del controllore H_inf, SS_CP
[acp bcp ccp dcp]=branch(SS_CP); %%% controllore H_inf in spazio di stato
acp=acp+p*eye(size(acp)); %%% shift inverso
SScontr=ss(acp,bcp,ccp,dcp); %% controllore H_inf dopo lo shift
contr=tf(SScontr); %% controllore H_inf come f.d.t. (ordine 15)
aus=ss(contr,'min'); %% realizzazione minima del controllore H_inf (ordine 13)
C=tf(aus); %% controllore H_inf (minima realizzazione) come f.d.t.

%%% Per il TELELABORATORIO %%%

STime=0.01;
save('C_inf','C')

%%% Spedire al TELELABORATORIO:
%%% (1) schema simulink "heli_t.mdl" 
%%% (2) file "C_inf.mat"
