%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Attuatore idraulico lineare AMT
% 
% Matlab ver. 6.5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all
close all

s=tf('s');

%========================================================================
% Definizione delle unità di misura del SI %

m=1; Km=1000*m; cm=0.01*m; mm=0.001*m;
secondi=1; msec=0.001*secondi; minuti=60*secondi; ora=60*minuti;
secondo=1;
Kg=1; N=1; rad=1;
Nm=N*m;
gradi=pi*rad/180;
g = 9.81*m/secondi^2;            	
Kgf=g;								
rpm=2*pi/60;
Amp=1;  V=1;
Ohm=V/Amp;
Pascal=1;
bar=10^5*Pascal;
lmin=0.001*m^3/(60*secondi);


%========================================================================
% Parametri meccanici 

m1=2*Kg;          % massa spingidisco

bc=0*20*N;            % attrito coulombiano 
Kbcd=1.1;           % coeff. x attrito primo distacco=b0*Kbcd;
bv=80*N*secondi/m;       % attrito viscoso 

corsanom=10*mm;
Fnom=500*N;
Ke=Fnom/corsanom;        % costante elastica molle


%========================================================================
% Parametri valvola (moog) e idraulici

Ki=50*bar/Amp;
Kq=1.5*bar/lmin;
tauv=16*msec;
Wnv=1200*rad/secondi;

dv=0.5;

Pnom=30*bar;
Af=Ke*corsanom/Pnom;            % area attiva spingidisco
Af=2*10^(-4)*m^2;             % valore rotondo

Pnom=Ke*corsanom/Af;

%========================================================================
% Verifica funzione di trasferimento 

a4=m1;
a3=bv+2*dv*Wnv*m1;
a2=Ke+2*dv*Wnv*bv+m1*Wnv^2+tauv*Kq*Af^2*Wnv^2;
a1=Wnv*(2*dv*Ke+bv*Wnv+Kq*Af^2*Wnv);
a0=Ke*Wnv^2;
b0=Ki*Af*Wnv^2;
Gs=b0/(a4*s^4+a3*s^3+a2*s^2+a1*s+a0);

% poli e zeri
[poli, zeri]=pzmap(Gs);

% guadagno statico
Gs0=Ki*Af/Ke;




%========================================================================
% Condizioni iniziali 
x0=0*m;
xp0=0.2*m/secondo; 


%========================================================================
% Dati di ingresso e asse dei tempi

Tcamp=0.2*msec;				% tempo di campionamento dati simulazione
Tfin=0.4*secondi;
tsim=[0:Tcamp:Tfin]';

% step di corrente (catena aperta)
Istep=0.5*Amp;

% riferimento di posizione (sistema retroazionato)
xr=10*mm+0*tsim;   


%========================================================================
% Simulazione 

tic
sim('FrizioneAMT_base',tsim)
toc


%========================================================================
% Plot 
lw=1.8;

figure(1)
clf
subplot(221)
plot(t,Pv/bar,'b','linewidth',lw)
hold on
plot(t,Px/bar,'m','linewidth',lw)
ylabel('Pressione Px(m) Pv(b) [bar]')
grid on
subplot(223)
plot(t,xpp,'r','linewidth',lw)
ylabel('Accelerazione xpp')
xlabel('Tempo')
grid on
subplot(222)
plot(t,xp,'g','linewidth',lw)
ylabel('Velocita` xp')
grid on
subplot(224)
plot(t,xrif/mm,'b','linewidth',lw)
hold on
plot(t,x/mm,'r','linewidth',lw)
ylabel('Posizione x(r), riferimento (b)  [mm]')
xlabel('Tempo')
grid on
zoom on

figure(2)
clf
h=plot(t,xr/mm,'b-.');
set(h,'linewidth',lw)
hold on
h=plot(t,x/mm,'r');
set(h,'linewidth',lw)
title('Posizione (r) e riferimento di posizione (b)')
ylabel('Posizione x [mm]')
xlabel('Tempo')
grid on
zoom on




