% file ES_TC.m
%
% Calcolo dell'evoluzione libera e forzata dello stato e dell'uscita
% di un sistema dinamico a tempo continuo (motore elettrico con carico)
% mediante la function ss2tf

clear all, close all
pack
format compact

La=0.1;
Ra=5;
Km=1;
J1=0.8;
b1=0.5;
k12=5;
b12=0;
J2=10;
b2=5;

A=[-Ra/La,       0,       -Km/La,       0,            0;...
        0,       0,            1,       0,            0;...
    Km/J1, -k12/J1, -(b1+b12)/J1,  k12/J1,       b12/J1;...
        0,       0,            0,       0,            1;...
        0,  k12/J2,       b12/J2, -k12/J2, -(b12+b2)/J2]

B=[1/La, 0; 0, 0; 0, 0; 0, 0; 0, -1/J2]

C=[0, 0, 0, 1, 0]

D=[0, 0]

x0=[0; 0.2; 0; 0.1; 0]

n=max(size(A));     % Dimensione del vettore di stato
p=size(B); p=p(2);  % Dimensione del vettore degli ingressi
q=size(C); q=q(1);  % Dimensione del vettore delle uscite

% Trasformate di Laplace degli ingressi
num_u1=1;
den_u1=[1 0];
num_u2=0;
den_u2=1;

% Calcolo della risposta nel tempo

% 1) Evoluzione libera 

% 1.a) Stato
[num_Xl,den_Xl]=ss2tf(A,x0,eye(n),zeros(n,1),1)
% 1.b) Uscita
[num_Yl,den_Yl]=ss2tf(A,x0,C,zeros(q,1),1)

% 2) Evoluzione forzata

% 2.a) Stato
[num_Xf1,den_Xf1]=ss2tf(A,B,eye(n),zeros(n,p),1)
[num_Xf2,den_Xf2]=ss2tf(A,B,eye(n),zeros(n,p),2)

for kk=1:n,
    num_Xf1tot(kk,:)=conv(num_u1,num_Xf1(kk,:));
    num_Xf2tot(kk,:)=conv(num_u2,num_Xf2(kk,:));
end
num_Xf1tot
num_Xf2tot
den_Xf1=conv(den_u1,den_Xf1)
den_Xf2=conv(den_u2,den_Xf2)


% 2.b) Uscita
[num_Yf1,den_Yf1]=ss2tf(A,B,C,D,1);
[num_Yf2,den_Yf2]=ss2tf(A,B,C,D,2);
for kk=1:q,
    num_Yf1tot(kk,:)=conv(num_u1,num_Yf1(kk,:));
    num_Yf2tot(kk,:)=conv(num_u2,num_Yf2(kk,:));
end
num_Yf1tot 
num_Yf2tot 
den_Yf1=conv(den_u1,den_Yf1)
den_Yf2=conv(den_u2,den_Yf2)

% Calcolo dei residui

[ r_Yl, p_Yl, k_Yl]=residue(    num_Yl, den_Yl)
[r_Yf1,p_Yf1,k_Yf1]=residue(num_Yf1tot, den_Yf1)
[r_Yf2,p_Yf2,k_Yf2]=residue(num_Yf2tot, den_Yf2)

% Tracciamento della risposta del sistema mediante simulazione
t=0:0.002:20;
u1=num_u1*t.^0;
u2=num_u2*t.^0;
[Y_u1,X_u1]=lsim(A,B(:,1),C,D(:,1),u1,t,x0);
[Y_u2,X_u2]=lsim(A,B(:,2),C,D(:,2),u2,t,x0);
Y_tot=Y_u1+Y_u2;

figure(1), plot(t,X_u1(:,1),t,X_u2(:,1)), title('X1')
figure(2), plot(t,X_u1(:,2),t,X_u2(:,2)), title('X2')
figure(3), plot(t,X_u1(:,3),t,X_u2(:,3)), title('X3')
figure(4), plot(t,X_u1(:,4),t,X_u2(:,4)), title('X4')
figure(5), plot(t,X_u1(:,5),t,X_u2(:,5)), title('X5')
figure(6), plot(t,Y_u1(:,1),t,Y_u2(:,1),t,Y_tot), title('Y')