%% Modelisation d un robot mobile

clc
clear
close
tic

%% Donn�es du probleme

% Parametres Inertielles
M_Body = 7.50;
M_Wheel = 2;
M_Leg = 1;
M_Bar_1 = 1;
M_Bar_2 = 1;

I_Body = 2;
I_Wheel = 0.1;
I_Leg = 0.06;
I_Bar_1 = 0.06;
I_Bar_2 = 0.06;

% Parametres Geometriques
global L_1 L_2 L_3 R_W l_B h_B hh L_B L_C L_E L_D phi_E phi_D

L_1 = 0.527;
L_2 = 0.409;
L_3 = 0.368;

R_W = 0.2;

hh = 0.05;

l_B = 0.580;
h_B = 0.378;

L_B = 0.319 - L_1*0.5;

L_C = L_1 * 0.5;
L_E = 0.079;
L_D = 0.179;
phi_E = deg2rad(9.5);
phi_D = deg2rad(237);

% Damping and Stiffness

% KK_BW = 10000;
% CC_BW = 1000;
% KK_WG = 50000;
% CC_WG = 5000;
nb = 5;

% PID Parameter of stabilization of turret

PID_p = 0;
PID_i = 0;
PID_d = 0;
%
Kp = 0.5;%8000000;
Kd = 100;%1380;
Ki = 0.5;%6200;

% Matrice Masse du systeme

M = diag([M_Body M_Body I_Body ...
M_Wheel M_Wheel I_Wheel ...
M_Leg M_Leg I_Leg ...
M_Bar_1 M_Bar_1 I_Bar_1 ...
M_Bar_2 M_Bar_2 I_Bar_2 ]);

g = -9.81;

mu1 = 0.9;
mu2 = 0.9;
e = 0.00002;
treshold = 0.0001;

% Obstacle points // Ground form

polyy = sin_To_Polyline(8.00,0.4,150,16,76);
Poly_Ground1 = [-2 0;25 0;25 0.2;25.4 0.2;25.4 0;25.7 0;25.7 0.2;26.1 0.2;26.1
0;26.4 0;26.4 0.2;26.8 0.2;26.8 0;27.1 0;27.1 0.2;27.5 0.2;27.5 0;27.8 0;27.8
0.2;28.2 0.2;28.2 0;28.5 0;38.5 -1;48.5 0;53.5 0;55 0.4;57 -0.2;58 -0.1;64.2 -
1;75.8 0.6;82.65 -0.56;88.5 0.6;115 -0.4;116 0;500 0];
Poly_Ground2 = [-2 0;30 0;30.4 0.2;30.8 0;50 0;50 0.2;50.5 0.2;50.5 0;70 0;70 -
0.25;71.4 -0.25;71.4 0;2000 0];
Poly_Ground3 = [-2 0;10 0;10 0.2;10.4 0.2;10.4 0;50 0];

Poly_Ground4 = [-4 0;16 0;polyy;76 0;2000 0];

Poly_Ground = Poly_Ground1;
%
% % Terrain : route plane puis descente
% Poly_Ground = [
% -10 0; % D�but du terrain (x = -10, y = 0)
% 10 0; % Fin de la route plane (x = 10, y = 0)
% 20 -5; % Descente (x = 20, y = -5)
% 30 -5; % Route plate apr�s descente (x = 30, y = -5)
% 40 0; % Remont�e (optionnelle, x = 40, y = 0)
% 50 0; % Continuit� de la route plane (x = 50, y = 0)
% 60 -2; % Nouvelle descente optionnelle
% 70 -2; % Continuit� en bas
% 80 0; % transition
% ]

ti = 0;
tf = 3;
delta = 0.001;
T_simul = ti:delta:tf;
dt=0.001;
% Vitesse roues
%tau1 = -400;
%tau2 = -400;
V_Car_km_h = 10;
V_Car = V_Car_km_h/3.6;
K_Car = 2;

previous_error = 0;

%% Conditions initiales
% q = [ x_Body y_Body theta_Body
% x_Wheel y_Wheel theta_Wheel
% x_Leg y_Leg theta_Leg
% x_Bar_1 y_Bar_1 theta_Bar_1
% x_Bar_2 y_Bar_2 theta_Bar_2]

% Positions
theta1(1) = 0.0 ;
theta2(1) = 0.0 ;
theta3(1) = deg2rad(48.54);
theta4(1) = - deg2rad(180 - 141.71);
theta5(1) = - deg2rad(180 - 140.31);

x1(1) = L_1 * cos(theta3(1)) - L_3 * cos(theta5(1)) - L_E * cos(phi_E);

y1(1) = R_W + L_1 * sin(theta3(1)) - L_3 * sin(theta5(1)) + L_E * sin(phi_E);
x2(1) = 0.0 ; y2(1) = R_W ;
x3(1) = 0.5 * L_1 * cos(theta3(1)) ; y3(1) = R_W + 0.5 * L_1 * sin(theta3(1));
x4(1) = x3(1) + L_B * cos(theta3(1)) - 0.5 * L_2 * cos(theta4(1));
y4(1) = y3(1) + L_B * sin(theta3(1)) - 0.5 * L_2 * sin(theta4(1));
x5(1) = L_1 * cos(theta3(1)) - 0.5 * L_3 * cos(theta5(1));
y5(1) = R_W + L_1 * sin(theta3(1)) - 0.5 * L_3 * sin(theta5(1));

% Vitesse
x1_p(1) = 0.0 ; y1_p(1) = 0.0; theta1_p(1) = 0.0;
x2_p(1) = 0.0 ; y2_p(1) = 0.0; theta2_p(1) = 0.0;
x3_p(1) = 0.0 ; y3_p(1) = 0.0; theta3_p(1) = 0.0;
x4_p(1) = 0.0 ; y4_p(1) = 0.0; theta4_p(1) = 0.0;
x5_p(1) = 0.0 ; y5_p(1) = 0.0; theta5_p(1) = 0.0;

%% MAIN LOOP
M1=[];
M2=[];
M3=[];
M4=[];
M5=[];
% Initialisation des figures et des courbes AVANT la boucle
figure;
% subplot(5,1,1); p1 = plot(nan, 'r', 'LineWidth', 1.5); title('�volution de
Theta1'); grid on;
% subplot(5,1,2); p2 = plot(nan, 'b', 'LineWidth', 1.5); title('�volution de
Theta2'); grid on;
% subplot(5,1,3); p3 = plot(nan, 'g', 'LineWidth', 1.5); title('�volution de
Theta3'); grid on;
% subplot(5,1,4); p4 = plot(nan, 'm', 'LineWidth', 1.5); title('�volution de
Theta4'); grid on;
% subplot(5,1,5); p5 = plot(nan, 'k', 'LineWidth', 1.5); title('�volution de
Theta5'); grid on;
subplot(2,1,1); p1 = plot(nan, 'r', 'LineWidth', 1.5); title('�volution ERP');
grid on;
subplot(2,1,2); p2 = plot(nan, 'b', 'LineWidth', 1.5); title('�volution ERV');
grid on;
ERV=[];
ERP=[];
for i = 1 : length(T_simul)

% Vecteur des coordonn�es generalis�es

 q = [x1(i) y1(i) theta1(i) ...
x2(i) y2(i) theta2(i) ...
x3(i) y3(i) theta3(i) ...
x4(i) y4(i) theta4(i) ...
x5(i) y5(i) theta5(i)]';

% Vecteur des vitesses generalis�es

q_p = [x1_p(i) y1_p(i) theta1_p(i) ...
x2_p(i) y2_p(i) theta2_p(i) ...
x3_p(i) y3_p(i) theta3_p(i) ...
x4_p(i) y4_p(i) theta4_p(i) ...
x5_p(i) y5_p(i) theta5_p(i)]';

% Les contraintes des liaisons et Jaccobienne des contraintes des liaisons

st1 = sin(theta1(i)) ; ct1 = cos(theta1(i));
st2 = sin(theta2(i)) ; ct2 = cos(theta2(i));
st3 = sin(theta3(i)) ; ct3 = cos(theta3(i));
st4 = sin(theta4(i)) ; ct4 = cos(theta4(i));
st5 = sin(theta5(i)) ; ct5 = cos(theta5(i));

[J,phi] = constraints_Jacobian(q);

phi1(i) = phi(1) ;
phi2(i) = phi(2) ;
phi3(i) = phi(3) ;
phi4(i) = phi(4) ;
phi5(i) = phi(5) ;
phi6(i) = phi(6) ;
phi7(i) = phi(7) ;
phi8(i) = phi(8) ;
phi9(i) = phi(9) ;
phi10(i) = phi(10) ;
%%

target_v=20;
error = target_v - theta1_p(i);
ERV= [ERV error];

% Calcul des termes du PID

PID_p = Kp * error; % Terme proportionnel
% PID_i = PID_i + Ki * error * dt; % Terme int�gral (accumulation
de l'erreur)
% PID_d = (error - previous_error) / dt; % Terme d�riv� (changement de
l'erreur)

% Calcul de la sortie PID

output = PID_p ;%+ PID_i + PID_d; % Somme des termes PID

% Mettre � jour la position du moteur (en fonction de la sortie)

theta1_p(i) = theta1_p(i) + output * dt; % Ajuster la position du moteur

% Sauvegarder la position pour l'affichage

v_history(i) = theta1_p(i);
 % Mettre � jour l'erreur pr�c�dente pour le prochain calcul
previous_error = error;
%
%
% %##################################################
target_Position=deg2rad(20);
error = target_Position - theta3(i);
ERP= [ERP error];

% Calcul des termes du PID

PID_p = Kp * error; % Terme proportionnel
PID_i = PID_i + Ki * error * dt; % Terme int�gral (accumulation de
l'erreur)
% PID_d = (error - previous_error) / dt; % Terme d�riv� (changement de
l'erreur)

% Calcul de la sortie PID

output2 = PID_p + PID_i ;%+ PID_d; % Somme des termes PID

% Mettre � jour la position du moteur (en fonction de la sortie)

theta1(i) = theta3(i) + output * dt; % Ajuster la position du moteur

% Sauvegarder la position pour l'affichage

Position_history(i) = theta3(i);

% Mettre � jour l'erreur pr�c�dente pour le prochain calcul

previous_error = error;
% ##########################################################

R4 = [ct4 -st4 0;st4 ct4 0;0 0 1];

tau1(i) = -K_Car * (V_Car - x1_p(i));

F_Body = [0 ;M_Body * g ;0] ;

F_Wheel = [0 ;M_Wheel * g ;0] ;%+ [ 0 ; 0 ; output];
F_Leg = [10 ;M_Leg * g ;0] ;
F_Bar_1 = [20 ;M_Bar_1 * g ;20]; %+[ 0 ; 0 ; output2];
F_Bar_2 = [0 ;M_Bar_2 * g ;0] ;

Fg = [F_Body ;F_Wheel;F_Leg;F_Bar_1;F_Bar_2];

h = M * q_p + delta * Fg;

% Stocker les forces
forces(i, :) = Fg;

% Stocker les vitesses

velocities(i, :) = q_p';

% Calculer et stocker les acc�l�rations (a = dq/dt)

if i > 1
accelerations(i, :) = (velocities(i, :) - velocities(i - 1, :)) / delta;
end

% Detection de contact
======================================================================

[yes_nott,ncc,nc,n,nbc,rG,err,Cnn] = Contact_Detection(q,Poly_Ground);

if yes_nott == 1
[Wn,D,E,b] = Wrenches_Matrix_2D(nb,ncc,nc,n,rG,nbc,err,e,treshold,delta);
mu = mu1 * eye(ncc);
H = [Wn D zeros(3*nb,ncc)];
N = [zeros(ncc,4*ncc);zeros(2*ncc,ncc*(1+2)) E;mu -E' zeros(ncc,ncc)];

invM_H=M\H;
invM_Jt = M\J';
invM_h=M\h;

G = H' * invM_H - H' * invM_Jt * inv(J * invM_Jt) * J * invM_H ;

s = b + H' * invM_h - H' * invM_Jt * inv(J * invM_Jt) *(J * invM_h);

A = G + N;
C = s;

xx = pathlcp(A,C);

impulse = - inv( J * inv(M) * J') * J * inv(M) * (H * xx + h);% + inv(J

* inv(M) * J') * gamma;

q_p = M \ (J' * impulse + H * xx + h);

end
if yes_nott == 0

K = [M -J';J zeros(10,10)];
B = [h;zeros(10,1)];

q_p = K\B;

end

x1_p(i+1) = q_p(1);
y1_p(i+1) = q_p(2);
theta1_p(i+1) = q_p(3);

x2_p(i+1) = q_p(4);
y2_p(i+1) = q_p(5);
theta2_p(i+1) = q_p(6);

x3_p(i+1) = q_p(7);
y3_p(i+1) = q_p(8);
theta3_p(i+1) = q_p(9);

x4_p(i+1) = q_p(10);
y4_p(i+1) = q_p(11);
theta4_p(i+1) = q_p(12);

x5_p(i+1) = q_p(13);
y5_p(i+1) = q_p(14);
theta5_p(i+1) = q_p(15);
 q_new(1) = x1(i) + delta * x1_p(i+1);
q_new(2) = y1(i) + delta * y1_p(i+1);
q_new(3) = theta1(i) + delta * theta1_p(i+1);

q_new(4) = x2(i) + delta * x2_p(i+1);

q_new(5) = y2(i) + delta * y2_p(i+1);
q_new(6) = theta2(i) + delta * theta2_p(i+1);

q_new(7) = x3(i) + delta * x3_p(i+1);

q_new(8) = y3(i) + delta * y3_p(i+1);
q_new(9) = theta3(i) + delta * theta3_p(i+1);

q_new(10) = x4(i) + delta * x4_p(i+1);

q_new(11) = y4(i) + delta * y4_p(i+1);
q_new(12) = theta4(i) + delta * theta4_p(i+1);

q_new(13) = x5(i) + delta * x5_p(i+1);

q_new(14) = y5(i) + delta * y5_p(i+1);
q_new(15) = theta5(i) + delta * theta5_p(i+1);

[J_new,Phi_new] = constraints_Jacobian(q_new);

dq_new = -(J_new'*inv(J_new*J_new'))*Phi_new;

% Mise � jour des positions et des angles

x1(i+1) = q_new(1) + dq_new(1);
y1(i+1) = q_new(2) + dq_new(2);
theta1(i+1) = q_new(3) + dq_new(3);
%M1 = [M1 theta1(i+1)];

x2(i+1) = q_new(4) + dq_new(4);

y2(i+1) = q_new(5) + dq_new(5);
theta2(i+1) = q_new(6) + dq_new(6);
%M2 = [M2 theta2(i+1)];

x3(i+1) = q_new(7) + dq_new(7);

y3(i+1) = q_new(8) + dq_new(8);
theta3(i+1) = q_new(9) + dq_new(9);
%M3 = [M3 theta3(i+1)];

x4(i+1) = q_new(10) + dq_new(10);

y4(i+1) = q_new(11) + dq_new(11);
theta4(i+1) = q_new(12) + dq_new(12);
%M4 = [M4 theta4(i+1)];

x5(i+1) = q_new(13) + dq_new(13);

y5(i+1) = q_new(14) + dq_new(14);
theta5(i+1) = q_new(15) + dq_new(15);
%M5 = [M5 theta5(i+1)];
% Appliquer une saturation entre -1 et 1 pour tous les angles avant
stockage
theta1(i+1) = max(-1, min(1, q_new(3) + dq_new(3)));
theta2(i+1) = max(-1, min(1, q_new(6) + dq_new(6)));
 theta3(i+1) = max(-1, min(1, q_new(9) + dq_new(9)));
theta4(i+1) = max(-1, min(1, q_new(12) + dq_new(12)));
theta5(i+1) = max(-1, min(1, q_new(15) + dq_new(15)));

% Mettre � jour les valeurs des angles dans les vecteurs de stockage
M1 = [M1 theta1(i+1)];
M2 = [M2 theta2(i+1)];
M3 = [M3 theta3(i+1)];
M4 = [M4 theta4(i+1)];
M5 = [M5 theta5(i+1)];

% % Mise � jour dynamique des courbes

% set(p1, 'XData', T_simul(1:i), 'YData', M1);
% set(p2, 'XData', T_simul(1:i), 'YData', M2);
% set(p3, 'XData', T_simul(1:i), 'YData', M3);
% set(p4, 'XData', T_simul(1:i), 'YData', M4);
% set(p5, 'XData', T_simul(1:i), 'YData', M5);
set(p1, 'XData', T_simul(1:i), 'YData', ERP);
set(p2, 'XData', T_simul(1:i), 'YData', ERV);

end

toc

%% Animation
===================================================================================
=
hold on
grid on
axis([-4 4 -4 4]);

v = VideoWriter('V30_T625_A020_W.avi');
open(v);

for i = 1 :150: length(T_simul)-3

cla
axis([x1(i)-1 x1(i)+1 -0.5 2]);
hold on
% Appliquer la saturation sur les angles utilis�s pour l'animation
theta1_anim = max(-pi/12, min(pi/12, theta1(i)));
theta3_anim = max(-pi/12, min(pi/12, theta3(i)));
theta4_anim = max(-pi/12, min(pi/12, theta4(i)));
theta5_anim = max(-pi/12, min(pi/12, theta5(i)));

% Matrices de rotation avec les angles satur�s

RR1 = [cos(theta1_anim) -sin(theta1_anim); sin(theta1_anim) cos(theta1_anim)];
RR3 = [cos(theta3_anim) -sin(theta3_anim); sin(theta3_anim) cos(theta3_anim)];
RR4 = [cos(theta4_anim) -sin(theta4_anim); sin(theta4_anim) cos(theta4_anim)];
RR5 = [cos(theta5_anim) -sin(theta5_anim); sin(theta5_anim) cos(theta5_anim)];
% RR1 = [cos(theta1(i)) -sin(theta1(i));
% sin(theta1(i)) cos(theta1(i))];
%
% RR3 = [cos(theta3(i)) -sin(theta3(i));
% sin(theta3(i)) cos(theta3(i))];
%
% RR4 = [cos(theta4(i)) -sin(theta4(i));
% sin(theta4(i)) cos(theta4(i))];
%
% RR5 = [cos(theta5(i)) -sin(theta5(i));
% sin(theta5(i)) cos(theta5(i))];

G1 = [x1(i) y1(i)]';
b11 = G1 + RR1*[0.5 * l_B 0.5 * h_B]';
b12 = G1 + RR1*[-0.5 * l_B 0.5 * h_B]';
b13 = G1 + RR1*[-0.5 * l_B -0.5 * h_B]';
b14 = G1 + RR1*[0.5 * l_B -0.5 * h_B]';

G3 = [x3(i) y3(i)]';
b31 = G3 + RR3*[0.5 * L_1 0.5 * hh]';
b32 = G3 + RR3*[-0.5 * L_1 0.5 * hh]';
b33 = G3 + RR3*[-0.5 * L_1 -0.5 * hh]';
b34 = G3 + RR3*[0.5 * L_1 -0.5 * hh]';

G4 = [x4(i) y4(i)]';
b41 = G4 + RR4*[0.5 * L_2 0.5 * hh]';
b42 = G4 + RR4*[-0.5 * L_2 0.5 * hh]';
b43 = G4 + RR4*[-0.5 * L_2 -0.5 * hh]';
b44 = G4 + RR4*[0.5 * L_2 -0.5 * hh]';

G5 = [x5(i) y5(i)]';
b51 = G5 + RR5*[0.5 * L_3 0.5 * hh]';
b52 = G5 + RR5*[-0.5 * L_3 0.5 * hh]';
b53 = G5 + RR5*[-0.5 * L_3 -0.5 * hh]';
b54 = G5 + RR5*[0.5 * L_3 -0.5 * hh]';

Poly_Body = [b11';b12';b13';b14';b11'];
Poly_Leg = [b31';b32';b33';b34';b31'];
Poly_Bar_1 = [b41';b42';b43';b44';b41'];
Poly_Bar_2 = [b51';b52';b53';b54';b51'];

drawPolyline(Poly_Ground,'k', 'lineWidth', 2);

drawPolyline(Poly_Body,'g', 'lineWidth', 2);

drawPolyline(Poly_Leg,'b', 'lineWidth', 2);
drawPolyline(Poly_Bar_1,'b', 'lineWidth', 2);
drawPolyline(Poly_Bar_2,'b', 'lineWidth', 2);

%drawCircle(x2(i),y2(i),R_W,4,6);
drawCircle(x2(i),y2(i),R_W);

drawPoint(x2(i),y2(i));
plot([x2(i) x2(i)+R_W*cos(theta2(i))],[y2(i) y2(i)
+R_W*sin(theta2(i))],'r','Linewidth',3);

frame = getframe(gcf);
writeVideo(v,frame);
 pause(0.01)
end

close(v);
% % Tracer les forces
% figure;
% plot(T_simul, forces(:, 1), 'r', 'LineWidth', 1.5);
% hold on;
% plot(T_simul, forces(:, 2), 'b', 'LineWidth', 1.5);
% xlabel('Temps (s)');
% ylabel('Forces (N)');
% legend('Force x1', 'Force y1');
% title('Forces g�n�ralis�es');
% grid on;
%
% % Tracer les vitesses
% figure;
% plot(T_simul, velocities(:, 1), 'r', 'LineWidth', 1.5);
% hold on;
% plot(T_simul, velocities(:, 2), 'b', 'LineWidth', 1.5);
% xlabel('Temps (s)');
% ylabel('Vitesses (m/s)');
% legend('Vitesse x1', 'Vitesse y1');
% title('Vitesses g�n�ralis�es');
% grid on;
%
% % Tracer les acc�l�rations
% figure;
% plot(T_simul, accelerations(:, 1), 'r', 'LineWidth', 1.5);
% hold on;
% plot(T_simul, accelerations(:, 2), 'b', 'LineWidth', 1.5);
% xlabel('Temps (s)');
% ylabel('Acc�l�rations (m/s�)');
% legend('Acc�l�ration x1', 'Acc�l�ration y1');
% title('Acc�l�rations g�n�ralis�es');
% grid on;
%
%