%% Start
clc
clear
close all
%% Control system data
% global b rd
velMax = 2387 *1.1* 2 * pi / 60;                        % velocità massima motore [rad/s]
%accMax = 30 * 9.8;                                 % accelerazione massima [rad/s^2]
Torque = 2.55;                                      % [Nm]

tauE = 1e-3;                                        % costante di tempo circuito elettrico motore [s]
iMax = 20.7*1.1;                                          % corrente massima [A]
Kc = 0.15;                                          % costante di coppia massima [N/A]
TorqueMax = Kc * iMax;
R = velMax * TorqueMax / (iMax)^2;                  % resistenza del curcuito elettrico
L = R * tauE;
VaMax = Kc * velMax;
rap_rid = 10.167;                                   %rapporto riduzione motore

%% anello di corrente

omegaC_i = 1000 * 2* pi;                             % [rad/s] pulsazione di crossover

%Kp_i = L * omegaC_i;                                % proporzionale del PID dell'anello di corrente [Wb * s/rad/A = H * s / A]
Kp_i = 5;
%Kp_i = 1;
%Ki_i = Kp_i / tauE;                                 % integrativo del PID dell'anello di corrente [Wb / A /rad = H / rad]    
%Ki_i = 0.1;
Ki_i=1;

%% anello di velocità

omegaC_v = 100 * 2 * pi;                             % [rad/s] pulsazione di crossover
Inertia = 230;                                       % [kg] inertia
accMaxM = TorqueMax / Inertia;       % [m / s^2] accelerazione massima
phiMarVel = deg2rad(87);                             % [rad] margine di fase

% Kp and Ki to be tuned, here possible starting values
%Kp_v = omegaC_v * Inertia / Kc * (tan(phiMarVel)/ sqrt(1 + (tan(phiMarVel))^2));       % proporzionale del PID dell'anello di velocità [rad * s * A / m]
 Kp_v = 5;
 % Kp_v = 1;
%Ki_v = omegaC_v^2 * Inertia / Kc / sqrt(1 + (tan(phiMarVel))^2);                       % integrativo del PID dell'anello di velocità [rad^2 * A / m]
 Ki_v = 1;

%% Vehicle data
mv = 83;          %Wheight of the whole vehicle (kg)
%mv = 110;

l = 0.800; 
w = 0.580;
rd = 0.1;          %Radius of driving wheel (m)
rc = 0.125/2;      %Radius of caster wheel (m)
b_caster = 0.07;   % Caster wheel offset (m)
Iv = mv * (l^2 + w^2)  / 12; %Moment of inertia of the vehicle (kg.m^2)
h_caster = 0.1;
h=0.20;              %Baricentre height
H=1;                 %Clamp height

ad = 0;           %Lenght between center of gravity of vehicle and driving wheel /x (m)
bd = 0.205;        %Lenght between center of gravity of vehicle and driving wheel /y (m)
a = 0.250;       %Lenght between centre of gravity of vehicle and joint of caster wheel /x (m)
b = 0.1875;      %Lenght between centre of gravity of vehicle and joint of caster wheel /y  (m)
ll=(a^2+b^2)^0.5;
zeta=atan(b/a);

ec=0.02132;     %thickness of caster wheel (m)
ed=0.033;       %thickness of driver wheel (m)

rhopoliu = 100;
mdw = rd^2 * pi * ed * rhopoliu + 1;
Idw_1 = 1/2 * mdw * rd^2;
Idw_2 = 1/12 * mdw * ed^2 + 1/4 * mdw * rd^2;

mcw = rc^2 * pi * ec * rhopoliu + 1;
Icw_1 = 1/2 * mcw * rc^2;
Icw_2 = 1/12 * mcw * ec^2 + 1/4 * mcw * rc^2;

%% Coefficienti di attrito
%Le funzioni per il calcolo dei coefficienti di attrito longitudinale e tresversale sono
%direttamente implementate nel SimuLink, in questo script vanno definiti
%solo i paramtri caratteristici delle funzioni dei coefficienti di attrito

f_d3 = 0.35846;
fa_d3 = 0.3694;

f_d4 = 0.2333;
fa_d4 = 0.28065;

fa_c = 0.27474;
f_c = 0.29564;

s1 = 0.18987;
s2 = 0.99533;

a1 = 0.18265;
a2 = 0.40857;

Mza = 32/3000 *(1200/2500/0.6);                                            %va moltiplicato con fa_d3 o fa_d4 a seconda della ruota a cui si riferisce quindi nel Simulink è presente un gain rispettivamente per la ruote 3 e 4
am1 = deg2rad(2.5);
am2 = deg2rad(5);

fm_fun = @(u) (u <= -am2 || u >= am2) * 0 + (u > -am2 && u <= -am1) * (- u* Mza/(am2 -am1) - Mza* am2/(am2 -am1)) + ...
         + (u > -am1 && u <= am1) * (Mza * u / am1) + (u > am1 && u < am2) * (Mza * u /(am1 -am2) - Mza * am2 /(am1 - am2)); 

figure
fplot(fm_fun, deg2rad([-40 40]),'linewidth',1.5)
grid on
title('M_z')
title('Coefficiente di attrito auto-allineamento')
xlabel('\alpha_i')
ylabel('f_m')

% -------------------------------------
% resistenza al rotolamento
kk = 0.00005;
kv = 1.5e-6;
%f_v = @(om) kk * om + kv * om^2;
%fplot(f_v)
% 
cccc = 1e20;

%coefficienti di attrito viscoso delle caster wheels
Att_C1=0.01;
Att_C2=0.01;
Att_C5=0.01;
Att_C6=0.01;

%% Normal reactions

F63MIR =@(u) (mv*h*u(5)+(mv*9.801/4)*(2*a-b_caster*(cos(u(1))+cos(u(2)))))/(4*a-b_caster*(cos(u(1))+cos(u(2)))+b_caster*(cos(u(3))+cos(u(4))));

F23MIR =@(u) (mv*9.801/4)-(mv*h*u(5)+(mv*9.801/4)*(2*a-b_caster*(cos(u(1))+cos(u(2)))))/(4*a-b_caster*(cos(u(1))+cos(u(2)))+b_caster*(cos(u(3))+cos(u(4))));

F43MIR =@(u) (1/(2*bd))*(mv*u(6)*h+((mv*h*u(5)+(mv*9.801/4)*(2*a-b_caster*(cos(u(1))+cos(u(2)))))/(4*a-b_caster*(cos(u(1))+cos(u(2)))+b_caster*(cos(u(3))+cos(u(4)))))*(2*bd-b_caster*(sin(u(3))+sin(u(4))))+((mv*9.801/4)-(mv*h*u(5)+(mv*9.801/4)*(2*a-b_caster*(cos(u(1))+cos(u(2)))))/(4*a-b_caster*(cos(u(1))+cos(u(2)))+b_caster*(cos(u(3))+cos(u(4)))))*(2*bd-b_caster*(sin(u(1))+sin(u(2)))));

F63UR5=@(u)(-a*u(7)/2 -u(5)*H -u(9) +(u(7)/4)*(2*a-b_caster*(cos(u(1))+cos(u(2)))))/(4*a-b_caster*(cos(u(1))+cos(u(2)))+b_caster*(cos(u(3))+cos(u(4))));

F23UR5 =@(u) (u(7)/4)-u(11);

F43UR5 =@(u) (1/(2*bd))*(u(8) - u(6)*H +u(11)*(2*bd-b_caster*(sin(u(3))+sin(u(4))))+u(12)*(2*bd-b_caster*(sin(u(1))+sin(u(2)))));
%% Parameters and Motion Setting
run UR5e_data.m
run UR5e_simmot.m
%% Dati sperimentali output
load('Feedback sperimentali\Test_IY_2_optrck.mat')                                               %scegliere i dati sperimentali ottenuti con OptiTrack

%Traslazione del sistema di riferimento

x_mir = x_mir-x_mir(1,1)*ones(length(x_mir),1);                            

z_mir = z_mir-z_mir(1,1)*ones(length(z_mir),1);

psi_mir = phi_mir*(pi/180)+(pi/2)*ones(length(phi_mir),1);

%Calcolo velocità sperimentale lungo Y

yd = diff(x_mir)./diff(time_2);

xd = (time_2(2:end)+time_2(1:end-1))/2;

%velocità sperimentale
figure
plot(xd,yd,'linewidth',1.5)
grid on
xlabel('tempo [s]')
ylabel('Velocità lungo Y [m/s]')
title('Velocità sperimentale MiR')


%traiettoria sperimentale
figure
plot(z_mir,x_mir,'linewidth',1.5)
xlim([-5 +5])
grid on
xlabel('X [m]')
ylabel('Y [m]')
title('Traiettoria sperimentale')

%% Caricamento spostamenti di set da Apirest

load('Set sperimentali\MiR\ExpDataSET_s.mat')                                                  %spostamenti sperimentali da Apirest

time_set = ExpDataSET.t';                                                  %tempo sperimentale da Apirest

traj=ones(length(ExpDataSET.t),2);                                         %prealloco spazio della traiettoria di set sperimentale

% Operazione di Timing: spostamento dei dati di OptiTrack lungo l'asse dei
% tempi per coordinare lo spostamento acquisito
%da Optitrack con lo spostamento acquisito da Apirest

u = 3;                                                                  %Numero di secondi per coordinare ExpDataSET2 con Test_IN_4_optrck

time_2 = time_2 + u*ones(length(time_2),1);                                %Traslazione del vettore dei tempi


traj(:,1) = -1*(ExpDataSET.x'-ExpDataSET.x(1,1)*ones(length(ExpDataSET.x'),1));

traj_interp_x = interp1(time_set,traj(:,1),time_2);                        %Calcolo degli spostamenti lungo X ottenuti da Apirest negli istanti acquisiti da Optitrack

traj(:,2) = -1*(ExpDataSET.y'-ExpDataSET.y(1,1)*ones(length(ExpDataSET.y'),1));

traj_interp_y = interp1(time_set,traj(:,2),time_2);                        %Calcolo degli spostamenti lungo Y ottenuti da Apirest negli istanti acquisiti da Optitrack

psi = pi*ones(length(ExpDataSET.psi'),1)+ExpDataSET.psi';
psi_interp = interp1(time_set,psi(:,1),time_2);                            %Calcolo delle rotazioni psi ottenute da Apirest negli istanti acquisiti da Optitrack

traj = ones(length(time_2),2);                                             %prealloco spazio degli spostamenti 

traj(:,1) = traj_interp_x;                                                 %spostamenti di set lungo X
traj(:,2) = traj_interp_y;                                                 %spostamenti di set lungo Y
psi = psi_interp;                                                          %rotazioni di set 

time_set = time_2-time_2(1,1)*ones(length(time_2),1);                      %riporto il dominio del tempo con origine in 0 secondi

figure
plot(traj(:,1),traj(:,2),'linewidth',1.5)                                  % plot traiettoria di set
title('Traiettoria di set')
xlabel('x [m]')
ylabel('y [m]')
grid on
xlim([min(traj(:,1))-1 max(traj(:,1))+1])
ylim([min(traj(:,2))-1 max(traj(:,2))+1])
axis equal

%% Caricamento velocità di set da Apirest
load('Set sperimentali\MiR\ExpDataSET_v.mat')

% Carico le componenti di velocità nel SdR relativo xyz    
xd_set = ExpDataSET.Vx;                                                    % velocità lungo x [m/s]

yd_set = ExpDataSET.Vy;                                                    % velocità lungo y [m/s]

psid_set = ExpDataSET.Psid;                                                % velocità di rotazione [rad/s]

time_set = ExpDataSET.t;                                                   % dominio del tempo da Apirest

Vx_interp = interp1(time_set,xd_set(:,1),time_2);                          % calcolo della velocità di set lungo x negli istanti temporali dei dati di OptiTrack traslati di u

Vy_interp = interp1(time_set,yd_set(:,1),time_2);                          % calcolo della velocità di set lungo y negli istanti temporali dei dati di OptiTrack traslati di u

Psid_interp = interp1(time_set,psid_set,time_2);                           % calcolo delle velocità angolari di set negli istanti temporali dei dati di OptiTrack traslati di u

% Assegno un nuovo nome alle variabili
xd_set = Vx_interp;
yd_set = Vy_interp;
psid_set = Psid_interp;

% Riporto l'asse dei tempi con origine a zero
time_set = time_2-time_2(1,1)*ones(length(time_2),1);

%%%%%%
Psi_start = psi_mir(1,1);
X_start = traj(1,1);
Y_start = traj(1,2);


Phi1_start=psi_mir(1,1);
Phi2_start=psi_mir(1,1);
Phi5_start=psi_mir(1,1);
Phi6_start=psi_mir(1,1);

% prealloco spazio dei tempi
time_v = time_set;
time_s = time_set;

% plot spostamenti, velocità e traiettoria di set
figure
subplot(2,1,1)                                                             % grafico spostamenti lungo x, y e rotazioni di set rispetto al sistema di riferimento fisso
title('Coordinate in funzione del tempo')
xlabel('tempo [s]')
ylabel('Coordinate [m]')
hold all
plot(time_set,traj(:,1),'linewidth',1.5)
plot(time_set,traj(:,2),'linewidth',1.5)
plot(time_set,psi,'linewidth',1.5)
legend('X_G_,_S_E_T','Y_G_,_S_E_T','\psi_S_E_T')
grid on


subplot(2,1,2)                                                             % grafico velocità lungo x, y e velocità angolare di set rispetto al sistema di riferimento fisso
hold all
plot(time_v,xd_set,'linewidth',1.5)
plot(time_v,yd_set,'linewidth',1.5)
plot(time_v,psid_set,'linewidth',1.5)
title('Velocità in funzione del tempo')
xlabel('tempo [s]')
ylabel('Velocità [m/s]')
legend('$\dot{x}_{G,SET}$','$\dot{y}_{G,SET}$','$\dot{\psi}_{SET}$','interpreter','latex')
grid on


%% Avvio simulazione
SimOut = sim('Modello_MiR_UR.slx'); 

%% Plot Grafici

t_out = SimOut.yout{3}.Values.Time;                                          % vettore tempi simulazione

%spostamento ottenuto integrando la velocità del centro di massa rispetto
%al sistema di riferimento fisso 
X_actual = SimOut.yout{3}.Values.Data;
Y_actual = SimOut.yout{2}.Values.Data;
psi_actual = SimOut.yout{1}.Values.Data;

Delta1 = SimOut.yout{6}.Values.Data;

Delta2 = SimOut.yout{7}.Values.Data;

Delta5 = SimOut.yout{8}.Values.Data;

Delta6 = SimOut.yout{9}.Values.Data;

F13 = SimOut.yout{10}.Values.Data(:,1);

F23 = SimOut.yout{10}.Values.Data(:,2);

F33 = SimOut.yout{10}.Values.Data(:,3);

F43 = SimOut.yout{10}.Values.Data(:,4);

F53 = SimOut.yout{10}.Values.Data(:,5);

F63 = SimOut.yout{10}.Values.Data(:,6);

% plot grafici confronto traiettoria ideale con traiettoria reale
figure
subplot(3,1,1)                                                             % confronto spostamenti lungo x
hold all
plot(time_set,z_mir,'linewidth',1.5)
plot(t_out,X_actual,'linewidth',1.5)
title('Confronto spostamenti lungo X')
xlabel('tempo [s]')
ylabel('spostamento [m]')
legend('X_G_,_E_X_P','X_G_,_S_I_M','location','best')
grid on


subplot(3,1,2)                                                             % confronto spostamenti lungo y
hold all
plot(time_set,x_mir,'linewidth',1.5)
plot(t_out,Y_actual,'linewidth',1.5)                                                     
title('Confronto spostamenti lungo Y')
xlabel('tempo [s]')
ylabel('spostamento [m]')
legend('Y_G_,_E_X_P','Y_G_,_S_I_M','location','best')
grid on


subplot(3,1,3)                                                             % confronto traiettorie
hold all
plot(time_set,psi_mir,'linewidth',1.5)                                     % traiettoria cinematica diretta                 
plot(t_out,psi_actual,'linewidth',1.5)                                     % traiettoria reale
title('Confronto posizioni angolari')
xlabel('tempo [s]')
ylabel('posizione angolare [rad]')
legend('\psi_E_X_P','\psi_S_I_M','location','best')
grid on

figure
plot(t_out,Delta1,'linewidth',1.5)
hold all
plot(t_out,Delta2,'linewidth',1.5)
plot(t_out,Delta5,'linewidth',1.5)
plot(t_out,Delta6,'linewidth',1.5)
legend('\delta_1','\delta_2','\delta_5','\delta_6')
title('Deflessione Delta Caster Wheels')
xlabel('tempo [s]')
ylabel('Deflessione [rad]')
grid on

figure
hold all
plot(t_out,F13,'LineWidth',1.5)
plot(t_out,F23,'LineWidth',1.5)
plot(t_out,F33,'LineWidth',1.5)
plot(t_out,F43,'LineWidth',1.5)
plot(t_out,F53,'LineWidth',1.5)
plot(t_out,F63,'LineWidth',1.5)
title('Reazioni Normali')
xlabel('tempo [s]')
ylabel('Reazione Normale [N]')
legend('F_1_,_N','F_2_,_N','F_3_,_N','F_4_,_N','F_5_,_N','F_6_,_N')
grid on

%% Animazione traiettoria reale

step = 2;                                                                    % numero step tra due pozioni consecutive
T0 = eye(4,4);                                                               % prealloco matrice di trasformazione omogenea per condizione iniziale
T0 = transl(0,0,0)*rpy2tr(0,0,pi/2);                                         % condizione iniziale MIR
B = zeros(4,4,step);                                                         % prealloco spazio matrice di trasformazione
increm=1;
% generazione matrice di trasormazione per l'intera traiettoria
for jj = 2:increm:length(X_actual(:,1))

    if jj==2

       T1 = transl(X_actual(jj,1),Y_actual(jj,1),0)*rpy2tr(0,0,psi_actual(jj,1));
       T = ctraj(T0,T1,step);
       B = T;
       T0 = T1;

    else

       T1 = transl(X_actual(jj,1),Y_actual(jj,1),0)*rpy2tr(0,0,psi_actual(jj,1));
       T = ctraj(T0,T1,step);
       A = cat(3,B,T);
       B = A;
       T0 = T1;
       if jj+10<=length(X_actual(jj,1))
           increm=10;
       else
           increm=1;
       end
    end
end

% generazione animazione traiettoria reale
figure
grid on
hold on
axis equal

plot(z_mir,x_mir,'linewidth',2)                                            % traiettoria ideale
plot(X_actual(:,1),Y_actual(:,1),'linewidth',2)                            % traiettoria reale

legend('traiettoria sperimentale','traiettoria simulata','Location','best')
%tranimate(B)
title('Traiettoria')
xlabel('X [m]')
ylabel('Y [m]')
ylim([min(traj(:,2))-1 max(traj(:,2))+1])

Thetad3_SET = SimOut.yout{4}.Values.Data;
Thetad3_OUT = SimOut.yout{11}.Values.Data;

Thetad4_SET = SimOut.yout{5}.Values.Data;
Thetad4_OUT = SimOut.yout{12}.Values.Data;

figure
subplot(2,1,1)
plot(t_out,Thetad3_SET,t_out,Thetad3_OUT,'linewidth',1.5)
title('confronto tra \omega_3_,_S_E_T e \omega_3_,_O_U_T')
legend('\omega_3_,_S_E_T','\omega_3_,_O_U_T','location','best')
xlabel('tempo [s]')
ylabel('\omega [rad/s]')
grid on
subplot(2,1,2)
plot(t_out,Thetad4_SET,t_out,Thetad4_OUT,'linewidth',1.5)
title('confronto tra \omega_4_,_S_E_T e \omega_4_,_O_U_T')
legend('\omega_4_,_S_E_T','\omega_4_,_O_U_T','location','best')
xlabel('tempo [s]')
ylabel('\omega [rad/s]')
grid on
