% Simulazione del sistema di controllo della frizione
% controllofrizione.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all; 
close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definizione delle unità di misura del SI
Kg=1;
sec=1; 
minuto=60*sec;
m=1; 
rad=1;
N=Kg*m/(sec^2); 
Pa=N/(m^2); 
bar=(10^5)*Pa; 
cm=10^(-2)*m;
gradi=pi*rad/180;
litri=(0.1*m)^3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definizione dei parametri del sistema
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Pm=50*bar;  						   % pressione di alimentazione  costante
R=0*bar;                        	% pressione di ritorno approssimata a 0

Kv=3*10^(-9)*m^3/(sec*Pa);       % costante di proporzionalità della valvola
Cm=1*10^(-10)*m^3/Pa;    	% capacità idraulica del cilindro
A=4*(cm^2);                     	% sezione del pistone
mp=0.3*Kg;                      	% massa del pistone
b=0.25*10^2*N*sec/m;             % coefficiente di attrito viscoso sul pistone
Km=0.8*10^4*N/m;                 % rigidità della molla


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calcolo funzione di trasferimento 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s = TF('s');

b0=A*Km*Kv;
a3=Cm*mp;
a2=Cm*b+Kv*mp;
a1=A^2+Cm*Km+Kv*b;
a0=Km*Kv;

SYSfriz = b0/(a3*s^3+a2*s^2+a1*s+a0);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Posizione poli funzione di trasferimento 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[num,den] = tfdata(SYSfriz,'v');

figure(1)
clf
plot(real(roots(den)),imag(roots(den)),'b*')
title('Posizione poli funzione di trasferimento')
xlabel ('Real')
ylabel ('Imag')
grid on
zoom on


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Risposta all'impulso ad anello aperto (antitrasformata) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Tfin=abs(5/max(real(roots(den))));
[Fm t]=impulse(SYSfriz,Tfin);

figure(2)
clf
plot(t,Fm,'r')
title ('Risposta all''impulso (antitrasformata)')
xlabel ('Tempo [sec]')
ylabel ('Fm [N]')
grid on
zoom on


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Risposta al gradino ad anello aperto 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Pstepwidth=10*bar;
Tfin=abs(5/max(real(roots(den))));
[Fmstep tstep]=step(Pstepwidth*SYSfriz,Tfin);

figure(3)
clf
plot(tstep,Fmstep,'r')
title (['Risposta al gradino (',num2str(Pstepwidth/bar,2),' bar) ad anello aperto'])
xlabel ('Tempo [sec]')
ylabel ('Fm [N]')
grid on
zoom on


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Risposta a un generico segnale 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Tfin=2*abs(5/max(real(roots(den))));
Tsim=[0:0.01:1]*Tfin;
Pin=20*bar+15*sin(2*(2*pi)*Tsim/Tfin)*bar;

[Fm,ts] = lsim(SYSfriz,Pin,Tsim);

Fmstatica=Pin*evalfr(SYSfriz,0);

figure(4)
clf
subplot(211)
plot(Tsim,Fm,'r')
hold on
plot(Tsim,Fmstatica,'b--')
title (['Risposta (r) al segnale di ingresso (b) ad anello aperto'])
ylabel ('Fm [N]')
grid on
subplot(212)
plot(Tsim,Pin/bar,'b')
ylabel ('P [bar]')
title (['Riferimento di pressione in ingresso'])
xlabel ('Tempo [sec]')
grid on
zoom on


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Diagrammi di Bode di Fm(s)/P(s) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(5)
clf
bode(SYSfriz)
grid on
zoom on


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Diagrammi di Bode Fm(s)/P(s) (grafico alternativo) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[Re3,Im3,w]=nyquist(SYSfriz);
Re=Re3(:);
Im=Im3(:);

MODdb=20*log10(sqrt(Re.^2+Im.^2));
FASEgradi=(atan2(Im,Re)-2*pi*(Im>0))/gradi;

figure(6)
clf
subplot(211)
semilogx(w,MODdb,'b')
title (['Diagrammi di Bode Fm(s)/P(s)'])
ylabel ('Ampiezza(b)[dB]')
axis([1 1000 -140 -60])
grid on
%
subplot(212)
semilogx(w,FASEgradi,'b')
ylabel ('Fase [gradi]')
xlabel ('w [rad/sec]')
axis([1 1000 -360 0])
grid on
zoom on


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Diagramma di Nyquist Fm(s)/P(s) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(7)
clf
plot(Re,Im,'b')
hold on
plot(Re,Im,'b.')
title ('Diagramma di Nyquist Fm(s)/P(s)')
xlabel ('Real')
ylabel ('Imag')
axis equal
grid on
zoom on


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calcolo guadagno KminTa per minimo tempo di assestamento 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sigmata=mean(real(roots(den)));
KminTa=-1/evalfr(SYSfriz,sigmata);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Luogo delle radici e verifica KminTa minimo tempo di assestamento 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K=10000*[0:0.01:1].^2;
Radici=rlocus(SYSfriz,K);
Poli=rlocus(SYSfriz,0);
RadiciTa=rlocus(SYSfriz,KminTa);
nr=size(Radici);
nr=nr(1);

figure(8)
clf
plot(real(Poli),imag(Poli),'b*')
hold on
for ii=[1:nr]
   plot(real(Radici(ii,:)),imag(Radici(ii,:)),'b')
end
plot(real(RadiciTa),imag(RadiciTa),'r*')
axis tight
title('Luogo delle radici')
xlabel ('Real')
ylabel ('Imag')
zoom on
grid on


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Definizione dei parametri del controllore
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tipoc=1;
switch tipoc
case 1
   % un possibile controllore che soddisfa le specifiche richieste
   Krc=122500;			% guadagno proporzionale 
   tau1=0.0229;		% costanti di rempo rete ritardatrice
   tau2=4.3922;
   % Cercare di sintetizzare lo stesso o analogo regolatore utilizzando 
   % gli strumenti base di controlli automatici: si aumenta il guadagno statico 
   % fino al soddisfacimento delle specifiche statiche e poi si inserisce una
   % rete ritardatrice per stabilizzare il sistema retroazionato. 
case 2
   % controllore con guadagno proporzionale x minimo Ta
   Krc=KminTa;			% guadagno proporzionale 
   tau1=0;				% costanti di rempo rete correttrice
   tau2=0;
case 3
   % il controllore con i vostri parametri
   Krc=1;				% guadagno proporzionale 
   tau1=0;				% costanti di rempo rete correttrice
   tau2=0;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Riferimento di forza (onda quadra) in ingresso
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Fmr=1000*N;				% riferimento di forza 
Tr=1*sec;				% periodo onda quadra 
DCr=50;     			% Duty cycle percentuale

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Condizioni iniziali
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0=0*cm;                        	% condizione iniziale sulla posizione 
P10=0*bar;                      	% condizione iniziale sulla pressione nella camera


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulazione
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% tempo di simulazione
tfin=1*sec;

% Simula lo schema a blocchi "frizionemdl" 
sim('controllofrizionemdl',tfin);           


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Grafici dei risultati 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% andamenti delle pressioni e della portata
figure(9)
clf
subplot(211)
plot(t,P1/bar,'r');
hold on
plot(t,P/bar,'b--');
title('Simulazioni controllo. Pressioni: di alimentazione (b--), camera (r)')
ylabel('[bar]');
grid on;
zoom on;
subplot(212)   
plot(t,Q/(litri/minuto),'g');
xlabel('Time (sec)');
ylabel('[l/min]');
title('Portata (Q)');
grid on;
zoom on;

% posizione e velocità del pistone
figure(10)
clf

subplot(211)   
plot(t,x*100,'b');
title('Simulazioni controllo. Posizione pistone')
ylabel('[cm]');
grid on;
zoom on;
subplot(212)   
plot(t,xdot,'r');
title('Velocità pistone')
xlabel('Time (sec)');
ylabel('[m/sec]');
grid on;
zoom on;

% forze "attive" sul pistone
figure(11)
clf
plot(t,P1*A,'b');
hold on
plot(t,Fm,'r');
xlabel('Time (sec)');
ylabel('[N]');
title('Simulazioni controllo. Forze attive sul pistone: idraulica (b), elastica (r)');
grid on;
zoom on;

% inseguimento del riferimento
figure(12)
clf
plot(t,Fmr,'b');
hold on
plot(t,Fm,'r');
xlabel('Time (sec)');
ylabel('[N]');
title('Simulazioni controllo. Forza elastica (r) e riferimento (b)');
grid on;
zoom on;

return


