clear all
close all
clc

global a 
a=0;


Ts=293;              %Temperatura ambiente durante la prova [K]
k=1.4;               % Rapporto calori specifici
Rg=287.053;          %Costante dell'aria [J/kgK]
Pa=0.101325*1e6;          %Pressione ambiente durante la prova
P0=Pa;
rho=Pa/(Rg*Ts); 
cd_ISO=0.87;
b=0.46;
bS=1/0.4;
Ps=0.45*1e6+Pa;
R0=0.72e-3;
cG=60000/rho;
fs=1000;
k=1.4;
dt=1/fs;
% VTubiA=0.52*pi*(0.002)^2+0.001; % CON SERBATOIO
VTubiA=6.47*pi*(0.002)^2; % SENZA SERBATOIO
VTubiB=6.47*pi*(0.002)^2; 
D=16e-3;
d=6e-3;
Aa=pi*D^2/4;
Ab=pi*(D^2-d^2)/4;
m_T=(0.75+0.13);
coeff=0.67;

s=500e-3;
l1=60e-3;

Flc=3.2693;
F_LVDT=4.53;


%Tolleranza sulla velocitÃ 
toll_v=0.0000005; 


%Parametri attrito
delta=0.7; 
vs=0.1; 
Fs=6; 
Fc1=1.5; 
c1=11; 
Fc2=2; 
c2=15; 

V0A=VTubiA*0.3;
V0B=VTubiB*0.49; 
% V0B=(s-l1)*Ab*0.3; 

%%
load 'Regolatore_PID_PA4_f15Hz_dV04_noserbatoio_1.txt'
ps1_V05_dV1_CameraB_chiusa_1=Regolatore_PID_PA4_f15Hz_dV04_noserbatoio_1;

PA=ps1_V05_dV1_CameraB_chiusa_1(:,1).*1e5+Pa; 
PB=ps1_V05_dV1_CameraB_chiusa_1(:,2).*1e5+Pa;   
QA=-ps1_V05_dV1_CameraB_chiusa_1(:,3); 
QB=-ps1_V05_dV1_CameraB_chiusa_1(:,4); 
Flc=ps1_V05_dV1_CameraB_chiusa_1(:,5);
X=ps1_V05_dV1_CameraB_chiusa_1(:,6)*1e-3; %Posizione, m
Xd=ps1_V05_dV1_CameraB_chiusa_1(:,7)*0.2601; %Velocità m/s
V=ps1_V05_dV1_CameraB_chiusa_1(:,8); %Segnale tensione di comando

time=(0:size(PA,1)-1)*1/fs;
[t,x] = ode45(@(t,x)Function(t,x,Aa,Ab,V0A,l1,F_LVDT,m_T,vs,delta,toll_v,c1,Fc1,Fs,c2,Fc2,time,Pa,PB),time,[PA(1); X(1);Xd(1)]);
%%

Vel_mod=x(:,3);
Acc_mod=diff(Vel_mod)/(t(2)-t(1));
Acc_mod=[Acc_mod; Acc_mod(end)];
Fp=(x(:,1)-Pa)*Aa-(PB-Pa)*Ab;

for i=1:length(Vel_mod)

v=Vel_mod(i);

if abs(v)-toll_v<=0  
cond=abs(Fp(i)-F_LVDT)<Fs;
Ff(cond)=Fp(i)-F_LVDT; 
cond=abs(Fp(i)-F_LVDT)>=Fs;
Ff(cond)=Fs*sign(Fp(i)-F_LVDT);

else
%2a) VelocitÃ  crescenti
cond=Acc_mod(i)>=0;    
Ff(cond)=c1*v+(Fc1+(Fs-Fc1)*exp(-(abs(v)./vs).^delta)).*sign(v);

%2a) VelocitÃ  decrescenti
cond=Acc_mod(i)<0;   
Ff(cond)=c2*v+Fc2*sign(v);

end

F_att(i)=(Ff);  
Fi(i)=m_T*Acc_mod(i);
end

Vel=diff(smooth(X,15))/dt;
tVel=time(1:end-1)+dt/2; 
tVel=[tVel tVel(end)+dt];
Vel=[Vel; Vel(end)];

Acc=diff(smooth(Xd,20))/dt;
Acc=[Acc; Acc(end)];

Fiexp=m_T*Acc;
Fpexp=(PA-Pa)*Aa-(PB-Pa)*Ab;
Ffexp=Fpexp-Fiexp-F_LVDT; 

figure
plot(Vel(1:13:end),Ffexp(1:13:end),'b-o','linewidth',1.5),hold on
plot(x(:,3),F_att,'r','linewidth',4)
xlabel('Velocity [$m/s$]','interpreter','latex'),ylabel('Friction Force [$N$]','interpreter','latex')
legend('Exp. Data','Num.')
grid on, zoom on,
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25) 
