%% EXAMEN DICIEMBRE 2019 - PROBLEMA 3: PARÁMETROS DINÁMICOS NEWTON-EULER
% Asignatura: Robótica (GITI - Escuela Superior de Ingenieros de Sevilla)
%
% Calcula los parámetros dinámicos del Eslabón 2 (varillas macizas):
%   - Masa m2
%   - Vector centro de gravedad s22
%   - Tensor de inercia baricéntrico I22

clear; clc;

fprintf('====================================================================\n');
fprintf('  EXAMEN DICIEMBRE 2019 - PROBLEMA 3: DINÁMICA NEWTON-EULER ESLABÓN 2\n');
fprintf('====================================================================\n\n');

syms L2A L2B R rho real positive

%% 1. DEFINICIÓN DE MASAS DE LAS 3 VARILLAS
% Varilla 1: horizontal inferior (L2B/2)
m1 = rho * (L2B / 2);
% Varilla 2: vertical intermedia (L2A)
m2_bar = rho * L2A;
% Varilla 3: horizontal superior (L2B/2)
m3 = rho * (L2B / 2);

% Masa total
m2 = simplify(m1 + m2_bar + m3);

fprintf('1. MASA TOTAL DEL ESLABÓN 2 (m2):\n');
disp(m2);
fprintf('   Forma simplificada: m2 = rho * (L2A + L2B)\n\n');

%% 2. CENTRO DE GRAVEDAD DEL ESLABÓN COMPUESTO
% Coordenadas de los CdM individuales respecto al inicio en Articulación 2:
x_c1 = L2B / 4;        y_c1 = 0;
x_c2 = L2B / 2;        y_c2 = L2A / 2;
x_c3 = 3 * L2B / 4;    y_c3 = L2A;

xc = simplify((m1*x_c1 + m2_bar*x_c2 + m3*x_c3) / m2);
yc = simplify((m1*y_c1 + m2_bar*y_c2 + m3*y_c3) / m2);
zc = sym(0);

fprintf('2. CENTRO DE MASAS RESPECTO A ARTICULACIÓN 2 (s12):\n');
s12 = [xc; yc; zc];
disp(s12);

fprintf('   Vector s22 en el sistema del eslabón {S2} (origen O2 en Artic. 3):\n');
s22 = [xc - L2B; yc - L2A; zc];
disp(s22);

%% 3. TENSOR DE INERCIA BARICÉNTTRICO (TEOREMA DE STEINER)
% Distancias relativas de cada varilla al centro de masas compuesto C2:
dx1 = x_c1 - xc;  dy1 = y_c1 - yc;
dx2 = x_c2 - xc;  dy2 = y_c2 - yc;
dx3 = x_c3 - xc;  dy3 = y_c3 - yc;

% Varilla 1 (horizontal en X): Ixx_c = 1/2 m R^2, Iyy_c = Izz_c = 1/12 m L^2
Ixx_1 = (1/2)*m1*R^2 + m1*dy1^2;
Iyy_1 = (1/12)*m1*(L2B/2)^2 + m1*dx1^2;
Izz_1 = (1/12)*m1*(L2B/2)^2 + m1*(dx1^2 + dy1^2);
Ixy_1 = - m1 * dx1 * dy1;

% Varilla 2 (vertical en Y): Iyy_c = 1/2 m R^2, Ixx_c = Izz_c = 1/12 m L^2
Ixx_2 = (1/12)*m2_bar*L2A^2 + m2_bar*dy2^2;
Iyy_2 = (1/2)*m2_bar*R^2 + m2_bar*dx2^2;
Izz_2 = (1/12)*m2_bar*L2A^2 + m2_bar*(dx2^2 + dy2^2);
Ixy_2 = sym(0);

% Varilla 3 (horizontal en X)
Ixx_3 = (1/2)*m3*R^2 + m3*dy3^2;
Iyy_3 = (1/12)*m3*(L2B/2)^2 + m3*dx3^2;
Izz_3 = (1/12)*m3*(L2B/2)^2 + m3*(dx3^2 + dy3^2);
Ixy_3 = - m1 * dx3 * dy3;

Ixx = simplify(Ixx_1 + Ixx_2 + Ixx_3);
Iyy = simplify(Iyy_1 + Iyy_2 + Iyy_3);
Izz = simplify(Izz_1 + Izz_2 + Izz_3);
Ixy = simplify(Ixy_1 + Ixy_2 + Ixy_3);

I22 = [ Ixx, -Ixy,    0;
       -Ixy,  Iyy,    0;
          0,    0,  Izz];

fprintf('3. MATRIZ DE INERCIA BARICÉNTRICA (I22):\n');
disp(I22);

%% 4. EJEMPLO NUMÉRICO CON DATOS TÍPICOS
L2A_num = 0.2;  % 20 cm
L2B_num = 0.6;  % 60 cm
R_num   = 0.05; % 5 cm (dado en examen)
rho_num = 1.5;  % 1.5 kg/m (dado en examen)

m2_val  = double(subs(m2,  [L2A, L2B, R, rho], [L2A_num, L2B_num, R_num, rho_num]));
s22_val = double(subs(s22, [L2A, L2B, R, rho], [L2A_num, L2B_num, R_num, rho_num]));
I22_val = double(subs(I22, [L2A, L2B, R, rho], [L2A_num, L2B_num, R_num, rho_num]));

fprintf('--- VALORES NUMÉRICOS PARA L2A=%.2fm, L2B=%.2fm, R=%.2fm, rho=%.2fkg/m ---\n', ...
    L2A_num, L2B_num, R_num, rho_num);
fprintf('Masa m2 = %.3f kg\n', m2_val);
fprintf('s22 = [%.3f; %.3f; %.3f] m\n', s22_val);
fprintf('I22 (kg*m^2) =\n');
disp(I22_val);