%% 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;
%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;
%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)

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.2;              %Baricentre 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)

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;


%% Forze di attrito
fa = 0.5;
f = 0.3;

fla =  fa  ;
fl = f  ;
s1 = 0.2;
s2 = 0.8;


fl_fun = @(u) (u <= -s2).* (-fl) + (u > -s2 & u <= -s1).*(u*(fl - fla)/(s2 - s1) + s2*(fl - fla)/(s2 - s1) - fl) +...
        + (u > -s1 & u <= s1).*(u * fla / s1) + (u > s1 & u <= s2).* (u * (fla - fl)/(s1 - s2) - s2 * (fla - fl)/(s1 - s2) + fl) + ...
        + (u> s2) * fl;
figure
fplot(fl_fun, [-1 1],'linewidth',1.5)
grid on
title('Coefficiente di attrito longitudinale')
xlabel('\sigma_i')
ylabel('f_l')
% --------------------------------------

fta = 1200/2500/0.6 * fa;
ft = 1200/2500/0.6 * f;

a1 = deg2rad(5);
a2 = deg2rad(25);

ft_fun = @(u) (u <= -a2).*(-ft) + (u > -a2 & u <= -a1).* (u*(ft - fta)/(a2 - a1) + a2*(ft - fta)/(a2 - a1) - ft) +...
              + (u > -a1 & u <= a1).* (u * fta / a1) + (u > a1 & u <= a2).* (u * (fta - ft)/(a1 - a2) - a2 * (fta - ft)/(a1 - a2) + ft) + ...
              + (u > a2).* ft;

figure
fplot(ft_fun, deg2rad([-40 40]),'linewidth',1.5)
title('Coefficiente di attrito tangenziale')
grid on
xlabel('\alpha_i')
ylabel('f_t')

% -------------------------------------
Mza = 32/3000 *fta;
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;
kk = 0.00005;
kv = 1.5e-6;
%f_v = @(om) kk * om + kv * om^2;
%fplot(f_v)
% 
cccc = 1e20;

Att_C=0.005;

%% Interpolazione Cubica Attrito Longitudinale
nvet = 23 ; %specificare i numeri di nodi ( −1) della partizione
inferior = -1; % estremo inferiore intervallo
superi = 1 ; % estremo superiore intervallo
zx = linspace(inferior,superi,10001); % definisco la griglia fine su cui valutare la spline
figure

n = nvet ;
x = linspace(inferior, superi, n+1); % in questo esercizio definisco i nodi di interpolazione tra loro equispaziati
Sx = spline(x ,fl_fun(x),zx) ; % valuto la spline cubica interpolante
hold on
grid on
box on
plot(zx,fl_fun(zx),'b',x,fl_fun(x),'ko',zx,Sx,'r','linewidth',1.5) % blu , curva originale ; cerchietti neri , nodi di interpolazione; rosso , spline
xlabel('\sigma_i');
ylabel('Coeff.di attrito longitudinale [-]')
title('Smussamento funzione di attrito longitudinale f_l(\sigma_i)')
legend('f_l(\sigma_i)','nodi','f_l_s(\sigma_i)','location','best') 
errrel = norm (fl_fun(zx)-Sx)/norm(fl_fun(zx));
disp (['n=', num2str(n), ' errore relativo norma infinito ' , num2str(errrel)])
%% Interpolazione Cubica Attrito Tagenziale
nvet = 20 ; %specificare i numeri di nodi ( −1) della partizione
inferior = -pi/2; % estremo inferiore intervallo
superi = pi/2; % estremo superiore intervallo
zy = linspace(inferior,superi,10001); % definisco la griglia fine su cui valutare la spline
figure

n = nvet ;
x = linspace(inferior, superi, n+1); % in questo esercizio definisco i nodi di interpolazione tra loro equispaziati
Sy = spline(x ,ft_fun(x),zy) ; % valuto la spline cubica interpolante
hold on
grid on
box on
plot(zy,ft_fun(zy),'b',x,ft_fun(x),'ko',zy,Sy,'r','linewidth',1.5) % blu , curva originale ; cerchietti neri , nodi di interpolazione; rosso , spline
xlabel('\alpha_i');
ylabel('Coeff. di attrito tangenziale')
title(' Smussamento funzione di attrito tangenziale f_t(\alpha_i) '  )
legend('f_t(\alpha_i)','nodi','f_t_s(\alpha_i)','location','best')
errrel = norm (ft_fun(zy)-Sy)/norm(ft_fun(zy));
disp (['n=', num2str(n), ' errore relativo norma infinito ' , num2str(errrel)])


%% Normal reactions

F63fun =@(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))));



F23fun =@(u) (mv*9.801/4)-u(7);



F43fun =@(u) (1/(2*bd))*(mv*u(6)*h+u(7)*(2*bd-b_caster*(sin(u(3))+sin(u(4))))+u(8)*(2*bd-b_caster*(sin(u(1))+sin(u(2)))));
%% Generazione traiettoria

% punto di partenza
X_start = input('Inserire coordinata x iniziale');
Y_start = input('Inserire coordinata y iniziale');
start = [X_start Y_start];

% parametri interpolazione
dt = 0.1;                                                % step temporale [s]
vmax = (velMax/rap_rid)*rd;                                                % velocità massima MIR [m/s]
accTime = 5;                                            % durata fase accelerazione (cambio direzione) [s]

% punti intermedi
n = input('Inserire numero di punti');
via = ones(n,2);
for ii = 1:(n-1)
    fprintf('inserire coordinate x del punto')
    disp(ii)
    x=input('');
    fprintf('inserire coordinate y del punto')
    disp(ii)
    y=input('');
    via(ii,:)=[x y];
end

% coordinate finali
x = input('inserire coordinata x finale');
y = input('inserire coordinata y finale');
via(end,1) = x;
via(end,2) = y;
Delta_start=0;

% generazione traiettoria e plot grafici 
traj = mstraj(via, vmax, [], start, dt, accTime); 


figure
plot(traj(:,1),traj(:,2),'linewidth',1.5)                               % plot traiettoria 
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

%% Interpolazione lineare per calcolo delle velocità 

% prealloco spazio degli spostamenti infinitesimi (intervalli tra due
% posizoni consecutive)
dx = ones(length(traj(:,1))-1,1);
dy = ones(length(traj(:,1))-1,1);
modulo = ones(length(traj(:,1))-1,1);

% calcolo spostamenti infinitesimi lungo x, y e risultante [m]
for ii=1:((length(traj(:,1)))-1)
    dx(ii,1) = abs(traj(ii+1,1)-traj(ii,1));
    dy(ii,1) = abs(traj(ii+1,2)-traj(ii,2));
    modulo(ii,1) = (dx(ii)^2+dy(ii)^2)^0.5;
end

% prealloco spazio posizione angolare
psi = ones(length(traj(:,1)),1);     

% prealloco spazio velocità media di traslazione e di rotazione
Xd_set = ones(length(dx(:,1)),1);
Yd_set = ones(length(dx(:,1)),1);

psid_set = ones(length(dx(:,1)),1);
cont=0;
var=0;
% calcolo posizione angolare [rad]
for ii = 2:length(traj(:,1))


    if traj(ii,1) > traj(ii-1,1) && traj(ii,2) >= traj(ii-1,2) || traj(ii,1) >= traj(ii-1,1) && traj(ii,2) > traj(ii-1,2)

       psi(ii,1) = ((asin((dy(ii-1))/modulo(ii-1,1)))); 

       elseif traj(ii,2) > traj(ii-1,2) && traj(ii,1) < traj(ii-1,1)

              psi(ii,1) = pi-(asin(dy(ii-1)/modulo(ii-1,1))); 

       elseif traj(ii,1) < traj(ii-1,1) && traj(ii,2) <= traj(ii-1,2) || traj(ii,1) <= traj(ii-1,1) && traj(ii,2) < traj(ii-1,2)

              psi(ii,1) = pi+abs((asin(dy(ii-1)/modulo(ii-1,1))));

       elseif traj(ii,2) < traj(ii-1,2) && traj(ii,1) > traj(ii-1,1)
       
              psi(ii,1) = 2*pi-(asin(dy(ii-1)/modulo(ii-1,1)));

       elseif traj(ii,1) == traj(ii-1,1) && traj(ii,2) == traj(ii-1,2) 
                  
                  var=var+1;
   end

    

% calcolo velocità media in ciascun intervallo [m/s]    
Xd_set(ii-1,1) = (traj(ii,1)-traj(ii-1,1))/0.1;                              % velocità media lungo x [m/s]
Yd_set(ii-1,1) = (traj(ii,2)-traj(ii-1,2))/0.1;                              % velocità media lungo y [m/s]
end
% generazione rotazione attorno a G
for tt = 2:length(traj(:,1))

    if traj(tt,1) == traj(tt-1,1) && traj(tt,2) == traj(tt-1,2) && cont==0
              
              traj11=traj(1:tt-1,1);
              traj12=traj((tt+var):end,1);
              traj21=traj(1:tt-1,2);
              traj22=traj((tt+var):end,2);
              Xd_set1=Xd_set(1:tt-1,1);
              Xd_set2=Xd_set((tt+var):end,1);
              Yd_set1=Yd_set(1:tt-1,1);
              Yd_set2=Yd_set((tt+var):end,1);
              psi1=psi(1:tt-1,1);
              psi2=psi((tt+var):end,1);
              rotation_length=150;
              courrent_ang_pos=(psi(tt-2))*(180/pi);
              courrent_ang_pos=num2str(courrent_ang_pos);
              fprintf('La posizione angolare del robot è')
              disp(courrent_ang_pos)
              rot=input('inserire rotazione in gradi');
              rot=rot*(pi/180);
              rotation=tpoly(0,rot,rotation_length)+psi1(end,1);
              
              traj1=[traj11;traj(tt-1,1)*ones(rotation_length,1);traj12];
              traj2=[traj21;traj(tt-1,2)*ones(rotation_length,1);traj22];
              traj=[traj1 traj2];
              Xd_set=[Xd_set1;zeros(rotation_length,1);Xd_set2];
              Yd_set=[Yd_set1;zeros(rotation_length,1);Yd_set2];
              psi=[psi1;rotation;psi2];
              cont=1;
    end
end


% assegno condizioni iniziali velocità e posizione angolare
Xd_set = [0;Xd_set;0];
Yd_set = [0;Yd_set;0];
Psi_start=pi/2;
psi(1,1) = Psi_start;

% calcolo velocità angolare [rad/s]
for ii = 2:(length(traj(:,1)))

psid_set(ii-1,1) = ((psi(ii,1)-psi(ii-1,1)))/0.1;

end

% assegno condizioni iniziali velocità angolare
psid_set = [0;psid_set;0]; 

% prealloco spazio dei tempi
time_v = ones(length(dx(:,1)),1);
time_s = ones(length(dx(:,1)),1);

% calcolo vettori dei tempi
for i = 2:length(traj(:,1))
time_v(i-1,1) = (i-1)*0.1-(0.1/2);                                           % vettore tempi per velocità [s]
time_s(i-1,1) = (i-1)*0.1;                                                   % vettore tempi per spostamenti [s]
end

% assegno istante iniziale ai vettori dei tempi
time_v = [0;time_v;time_s(end,1)];
time_s = [0;time_s];

% 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_s,traj(:,1),'linewidth',1.5)
plot(time_s,traj(:,2),'linewidth',1.5)
plot(time_s,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('AMR_Model_Simulink_v07_nocaster_Traj.slx'); 
SimOut = sim('Modello_MiR_6ruote.slx'); 

%% Plot Grafici

t_out = SimOut.tout;                                          % 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_s,traj(:,1),'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_,_S_E_T','X_G_,_O_U_T','location','best')
grid on


subplot(3,1,2)                                                             % confronto spostamenti lungo y
hold all
plot(time_s,traj(:,2),'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_,_S_E_T','Y_G_,_O_U_T','location','best')
grid on


subplot(3,1,3)                                                             % confronto traiettorie
hold all
plot(time_s,psi,'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_S_E_T','\psi_O_U_T','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(traj(:,1),traj(:,2),'linewidth',1.5)                                  % traiettoria ideale                                         
plot(X_actual(:,1),Y_actual(:,1),'linewidth',1.5)                          % traiettoria reale

legend('traiettoria ideale','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