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

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.3;
f = 0.2;

FLa = mv * 9.8 * fa / 2 ;
FL = mv * 9.8 *f / 2 ;
s1 = 0.2;
s2 = 0.8;

Fx = @(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(Fx, [-1 1],'linewidth',1.5)
hold on
plot(-s2,Fx(-s2),'r o',-s1,Fx(-s1),'g o',s1,Fx(s1),'g o',s2,Fx(s2),'r o','linewidth',1.5)
legend('F_i_,_\lambda_i(\sigma_i)','F_i_,_\lambda_i(-s2)','F_i_,_\lambda_i(-s1)','F_i_,_\lambda_i(s1)','F_i_,_\lambda_i(s2)')
grid on
title('Forza longitudinale')
xlabel('\sigma_i [-]')
ylabel('F_i_,_\lambda_i [N]')
xticks([ -s2 -s1 s1 s2])
xticklabels({'-s2', '-s1', 's1', 's2'})
yticks([ Fx(-s1) Fx(-s2) Fx(s2) Fx(s1)])
yticklabels({'-F_\lambda_,_s_t','-F_\lambda_,_{inst}','F_\lambda_,_{inst}','F_\lambda_,_s_t'})
ylim([-140 140])
% --------------------------------------
FTa = 1200/2500/0.6 * 9.8 * mv * fa / 2;
FT = 1200/2500/0.6 * 9.8 * mv * f / 2;
a1 = deg2rad(5);
a2 = deg2rad(25);

Fy = @(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(Fy, deg2rad([-90 90]),'linewidth',1.5)
hold on
plot(-a2,Fy(-a2),'r o',-a1,Fy(-a1),'g o',a1,Fy(a1),'g o',a2,Fy(a2),'r o','linewidth',1.5)
legend('F_i_,_\mu_i(\alpha_i)','F_i_,_\mu_i(-a2)','F_i_,_\mu_i(-a1)','F_i_,_\mu_i(a1)','F_i_,_\mu_i(a2)')
title('Forza trasversale')
xlabel('\alpha_i [rad]')
ylabel('F_i_,_\mu_i [N]')
grid on
ylim([-110 110])
xticks([ -a2 -a1 a1 a2])
xticklabels({'-a2', '-a1', 'a1', 'a2'})
xlim([-pi/4 pi/4])
yticks([Fy(-a1) Fy(-a2) Fy(a2) Fy(a1)])
yticklabels({'-F_\mu_,_s_t','-F_\mu_,_{inst}','F_\mu_,_{inst}','F_\mu_,_s_t'})
% -------------------------------------
Mza = 32/3000 *FTa;
am1 = deg2rad(2.5);
am2 = deg2rad(5);

Mz = @(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(Mz, deg2rad([-40 40]),'linewidth',1.5)
hold on
plot(-am2,Mz(-am2),'r o',-am1,Mz(-am1),'g o',am1,Mz(am1),'g o',am2,Mz(am2),'r o','linewidth',1.5)
legend('M_i(\alpha_i)','M_i(-am2)','M_i(-am1)','M_i(am1)','M_i(am2)')
grid on
xlabel('\alpha_i [rad]')
ylabel('M_i [Nm]')
title('Momento di auto-allineamento')
ylim([-1.1 1.1])
xticks([ -am2 -am1 am1 am2])
xticklabels({'-am2', '-am1', 'am1', 'am2'})
yticks([ Mz(-am1) Mz(-am2) Mz(am1)])
yticklabels({'-M_{st}','M_{inst}','M_{st}'})
xlim([deg2rad(-15) deg2rad(15)])
% -------------------------------------
% resistenza al rotolamento
kk = 0.00005;
kv = 1.5e-6;
%f_v = @(om) kk * om + kv * om^2;
%fplot(f_v)
% 
cccc = 1e20;


%% 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);
nr = 0;

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;

% 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
%% 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 (funziona una volta scelti 2 punti con le stesse coordinate)

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(1,1) = pi/2;

% 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('Modello_MiR_2ruote.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;

% 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                                                  % traiettoria ideale
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

%% 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(:,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,Y_actual,'linewidth',1.5)                                    % traiettoria reale

legend('traiettoria ideale', 'traiettoria simulata','Location','best')
%tranimate(B)
title('Traiettoria')
xlabel('X [m]')
ylabel('Y [m]')
 xlim([min(traj(:,1))-1 max(traj(:,1))+1])
 ylim([min(traj(:,2))-1 max(traj(:,2))+1])
axis equal
%% Plot altre variabili
Thetad3_SET = SimOut.yout{6}.Values.Data(:,2);
Thetad3_OUT = SimOut.yout{6}.Values.Data(:,1);

Thetad4_SET = SimOut.yout{7}.Values.Data(:,2);
Thetad4_OUT = SimOut.yout{7}.Values.Data(:,1);

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')
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')
xlabel('tempo [s]')
ylabel('\omega [rad/s]')
grid on

%% Plot modello slittamento
vmax1 = vmax;

Fc_pos = @(x,y) ((((x-y)/abs(x+1/cccc))*(((x-y)/abs(x+1/cccc))<=1 )+ 1*(((x-y)/abs(x+1/cccc))>1))*(x*y>=0 && x>=0) + (((x-y)/abs(y+1/cccc))*(((x-y)/abs(y+1/cccc))<=1 ))*(x*y>0 && x<0)+ (x*y<0)*(x/abs(x+1/cccc)))*(0.02<(x^2+y^2)) + (10*(x-y)*(10*(x-y)<1) +1*(10*(x-y)>=1))*(0.02>=(x^2+y^2));
Fc_neg = @(x,y) ((((x-y)/abs(y+1/cccc))*(((x-y)/abs(y+1/cccc))>=-1)+ (-1)*(((x-y)/abs(y+1/cccc))<-1))*(x*y>=0 && y>=0) + (((x-y)/abs(x+1/cccc))*(((x-y)/abs(x+1/cccc))>=-1))*(x*y>0 && y<0) + (x*y<0)*(x/abs(x+1/cccc)) )*(0.02<(x^2+y^2)) +(10*(x-y)*(10*(x-y)>-1) -1*(10*(x-y)<=-1))*(0.02>=(x^2+y^2)); 

x=linspace(-vmax1,vmax1,100)';
y=linspace(-vmax1,vmax1,100)';

Fc_pos_sol=ones(100,100);

Fc_neg_sol=ones(100,100);

for ii=1:100
for jj=1:100

    Fc_pos_sol(ii,jj)=Fc_pos(x(ii,1),y(jj,1));

    Fc_neg_sol(ii,jj)=Fc_neg(x(ii,1),y(jj,1));
end
end

Fc_sol=zeros(100,100);
Fc_sol_NL_MOD2= (Fc_pos_sol>=0).*Fc_pos_sol + (Fc_pos_sol<0).*Fc_neg_sol + Fc_sol;

figure
surf(y,x,Fc_sol_NL_MOD2)
ylabel('\omega_R*r [m/s]')
xlabel('V_C_,_x [m/s]')
zlabel('Slip factor')
title('Modello di Delft linearizzato intorno allo zero ')
