clear all
close all
clc


load Regolatore_PID_PA1_f1Hz_dV04_noserbatoio_2.txt
Regolatore_PID_PB1_f1Hz_dV04_1=Regolatore_PID_PA1_f1Hz_dV04_noserbatoio_2;

load Regolatore_PID_PA1_f15Hz_dV04_noserbatoio_2.txt
Regolatore_PID_PB1_f15Hz_dV04_1=Regolatore_PID_PA1_f15Hz_dV04_noserbatoio_2;

load Regolatore_PID_PA1_f2Hz_dV04_noserbatoio_2.txt
Regolatore_PID_PB1_f2Hz_dV04_1=Regolatore_PID_PA1_f2Hz_dV04_noserbatoio_2;

load Regolatore_PID_PA1_f25Hz_dV04_noserbatoio_2.txt
Regolatore_PID_PB1_f25Hz_dV04_1=Regolatore_PID_PA1_f25Hz_dV04_noserbatoio_2;

load Regolatore_PID_PA1_f3Hz_dV04_noserbatoio_2.txt
Regolatore_PID_PB1_f3Hz_dV04_1=Regolatore_PID_PA1_f3Hz_dV04_noserbatoio_2;

load Regolatore_PID_PA1_f35Hz_dV04_noserbatoio_2.txt
Regolatore_PID_PB1_f35Hz_dV04_1=Regolatore_PID_PA1_f35Hz_dV04_noserbatoio_2;

pa=101325;
cG=1.2047/60000;
T=293;
Rg=287.053;
fs=1000;
dt=1/fs;
m_T=0.75+0.13;
F_LVDT=4.53;
D=16e-3;
d=6e-3;
A1=pi*D^2/4;
A2=pi*(D^2-d^2)/4;


pA1=Regolatore_PID_PB1_f1Hz_dV04_1(:,1)*1e5+pa;
pB1=Regolatore_PID_PB1_f1Hz_dV04_1(:,2)*1e5+pa;
x1=Regolatore_PID_PB1_f1Hz_dV04_1(:,6)*1e-3;
v1=Regolatore_PID_PB1_f1Hz_dV04_1(:,7)*0.2601;
t1=[1:length(x1)]*dt;
vc1=diff(smooth(x1,30))/dt;
tv1=t1(1:end-1)+dt/2;
tv1=[tv1 tv1(end)+dt];
vc1=[vc1; vc1(end)];
a1=diff(smooth(v1,20))/dt;
ta1=t1(1:end-1)+dt/2;
ta1=[ta1 ta1(end)+dt];
a1=[a1; a1(end)];
Fi1=m_T*a1;
Fp1=(pA1-pa)*A1-(pB1-pa)*A2;
Ff1=Fp1-Fi1-F_LVDT;

pA15=Regolatore_PID_PB1_f15Hz_dV04_1(:,1)*1e5+pa;
pB15=Regolatore_PID_PB1_f15Hz_dV04_1(:,2)*1e5+pa;
x15=Regolatore_PID_PB1_f15Hz_dV04_1(:,6)*1e-3;
v15=Regolatore_PID_PB1_f15Hz_dV04_1(:,7)*0.2601;
t15=[1:length(x15)]*dt;
vc15=diff(smooth(x15,30))/dt;
tv15=t15(1:end-1)+dt/2;
tv15=[tv15 tv15(end)+dt];
vc15=[vc15; vc15(end)];
a15=diff(smooth(v15,20))/dt;
ta15=t15(1:end-1)+dt/2;
ta15=[ta15 ta15(end)+dt];
a15=[a15; a15(end)];
Fi15=m_T*a15;
Fp15=(pA15-pa)*A1-(pB15-pa)*A2;
Ff15=Fp15-Fi15-F_LVDT;

pA2=Regolatore_PID_PB1_f2Hz_dV04_1(:,1)*1e5+pa;
pB2=Regolatore_PID_PB1_f2Hz_dV04_1(:,2)*1e5+pa;
x2=Regolatore_PID_PB1_f2Hz_dV04_1(:,6)*1e-3;
v2=Regolatore_PID_PB1_f2Hz_dV04_1(:,7)*0.2601;
t2=[1:length(x2)]*dt;
vc2=diff(smooth(x2,30))/dt;
tv2=t2(1:end-1)+dt/2;
tv2=[tv2 tv2(end)+dt];
vc2=[vc2; vc2(end)];
a2=diff(smooth(v2,20))/dt;
ta2=t2(1:end-1)+dt/2;
ta2=[ta2 ta2(end)+dt];
a2=[a2; a2(end)];
Fi2=m_T*a2;
Fp2=(pA2-pa)*A1-(pB2-pa)*A2;
Ff2=Fp2-Fi2-F_LVDT;

pA25=Regolatore_PID_PB1_f25Hz_dV04_1(:,1)*1e5+pa;
pB25=Regolatore_PID_PB1_f25Hz_dV04_1(:,2)*1e5+pa;
x25=Regolatore_PID_PB1_f25Hz_dV04_1(:,6)*1e-3;
v25=Regolatore_PID_PB1_f25Hz_dV04_1(:,7)*0.2601;
t25=[1:length(x25)]*dt;
vc25=diff(smooth(x25,30))/dt;
tv25=t25(1:end-1)+dt/2;
tv25=[tv25 tv25(end)+dt];
vc25=[vc25; vc25(end)];
a25=diff(smooth(v25,20))/dt;
ta25=t25(1:end-1)+dt/2;
ta25=[ta25 ta25(end)+dt];
a25=[a25; a25(end)];
Fi25=m_T*a25;
Fp25=(pA25-pa)*A1-(pB25-pa)*A2;
Ff25=Fp25-Fi25-F_LVDT;

pA3=Regolatore_PID_PB1_f3Hz_dV04_1(:,1)*1e5+pa;
pB3=Regolatore_PID_PB1_f3Hz_dV04_1(:,2)*1e5+pa;
x3=Regolatore_PID_PB1_f3Hz_dV04_1(:,6)*1e-3;
v3=Regolatore_PID_PB1_f3Hz_dV04_1(:,7)*0.2601;
t3=[1:length(x3)]*dt;
vc3=diff(smooth(x3,30))/dt;
tv3=t3(1:end-1)+dt/2;
tv3=[tv3 tv3(end)+dt];
vc3=[vc3; vc3(end)];
a3=diff(smooth(v3,20))/dt;
ta3=t3(1:end-1)+dt/2;
ta3=[ta3 ta3(end)+dt];
a3=[a3; a3(end)];
Fi3=m_T*a3;
Fp3=(pA3-pa)*A1-(pB3-pa)*A2;
Ff3=Fp3-Fi3-F_LVDT;

pA35=Regolatore_PID_PB1_f35Hz_dV04_1(:,1)*1e5+pa;
pB35=Regolatore_PID_PB1_f35Hz_dV04_1(:,2)*1e5+pa;
x35=Regolatore_PID_PB1_f35Hz_dV04_1(:,6)*1e-3;
v35=Regolatore_PID_PB1_f35Hz_dV04_1(:,7)*0.2601;
t35=[1:length(x35)]*dt;
vc35=diff(smooth(x35,30))/dt;
tv35=t35(1:end-1)+dt/2;
tv35=[tv35 tv35(end)+dt];
vc35=[vc35; vc35(end)];
a35=diff(smooth(v35,20))/dt;
ta35=t35(1:end-1)+dt/2;
ta35=[ta35 ta35(end)+dt];
a35=[a35; a35(end)];
Fi35=m_T*a35;
Fp35=(pA35-pa)*A1-(pB35-pa)*A2;
Ff35=Fp35-Fi35-F_LVDT;

figure
plot(vc1(1:13:end),Ff1(1:13:end),'-o','LineWidth',1.5,'markersize',7),hold on
plot(vc15(1:13:end),Ff15(1:13:end),'-o','LineWidth',1.5,'markersize',7),hold on
plot(vc2(1:13:end),Ff2(1:13:end),'-o','LineWidth',1.5,'markersize',7),hold on
plot(vc25(1:13:end),Ff25(1:13:end),'-o','LineWidth',1.5,'markersize',7),hold on
plot(vc3(1:13:end),Ff3(1:13:end),'-o','LineWidth',1.5,'markersize',7),hold on
plot(vc35(1:13:end),Ff35(1:13:end),'-o','LineWidth',1.5,'markersize',7),hold on
xlabel('Velocity [$m/s$]','interpreter','latex'),ylabel('Friction Force [$N$]','interpreter','latex')
legend('1 [$Hz$]','1.5 [$Hz$]','2 [$Hz$]','2.5 [$Hz$]','3 [$Hz$]','3.5 [$Hz$]','interpreter','latex')
grid on, zoom on,
xlim([-0.4,0.4]),ylim([-7,7])
set(gca,'FontName','Times New Roman'),set(gca,'FontSize',25) %set(gca,'FontSize',32)

