%% =========================================================================
% EXAMEN ROBÓTICA GITI - TERCERA CONVOCATORIA (29 OCTUBRE 2025) - PROBLEMA 3
% =========================================================================
% Problema 1: Modelo Matricial M(q), V(q,qd), G(q), Linealización y PID
% Problema 2: Parámetros Dinámicos Newton-Euler del Eslabón 2 (Manivela)
% =========================================================================

clear; clc; close all;
fprintf('=================================================================\n');
fprintf('  EXAMEN ROBÓTICA GITI - OCTUBRE 2025 - PROBLEMA 3 (DINÁMICA/CONTROL)\n');
fprintf('=================================================================\n\n');

%% PARTE 1: MODELO DINÁMICO MATRICIAL Y CONTROL PID
fprintf('--- PARTE 1: MODELADO MATRICIAL Y CONTROL PID ---\n');
syms q1 q2 qd1 qd2 qdd1 qdd2 real
syms m1 m2 m3 m4 g real
syms T1 T2 real

% Ecuaciones dadas:
% eq1: (m1 + 2*m2*cos(q2))*qdd1 + (m3 + m2*cos(q2))*qdd2 - m2*sin(q2)*qd1*qd2 - m2*sin(q2)*(qd1+qd2)*qd2 = T1
% eq2: (m3 + m2*cos(q2))*qdd1 + m4*qdd2 - m2*sin(q2)*qd1^2 + 2*g*sin(q2) = T2

% Matriz de inercia M(q):
M = [m1 + 2*m2*cos(q2), m3 + m2*cos(q2);
     m3 + m2*cos(q2),   m4];

% Vector de Coriolis y centrífugos V(q, qd):
% Eje 1: -m2*sin(q2)*(2*qd1*qd2 + qd2^2)
% Eje 2: -m2*sin(q2)*qd1^2
V = [-m2*sin(q2)*(2*qd1*qd2 + qd2^2);
     -m2*sin(q2)*qd1^2];

% Vector gravitatorio G(q):
G = [0;
     2*g*sin(q2)];

fprintf('Matriz de Inercia M(q):\n');
disp(M);
fprintf('Vector de Coriolis y Centrífugos V(q, qd):\n');
disp(V);
fprintf('Vector Gravitatorio G(q):\n');
disp(G);

% Valores numéricos del problema:
m1_val = 1.0;
m2_val = 1.5;
m3_val = 2.0;
m4_val = 2.5;
g_val  = 9.81;

fprintf('\nValores numéricos asignados:\n');
fprintf('m1 = %.1f kg, m2 = %.1f kg, m3 = %.1f kg, m4 = %.1f kg, g = %.2f m/s^2\n', ...
    m1_val, m2_val, m3_val, m4_val, g_val);

% Modelos aproximados desacoplados:
% Eje 1: J1(q2) = m1 + 2*m2*cos(q2). Máxima inercia: cos(q2)=1 -> J1_max = 1 + 2*(1.5) = 4.0 kg*m^2
J1_max = m1_val + 2*m2_val;
% Eje 2: J2 = m4 = 2.5 kg*m^2 (con compensación de gravedad G2 = 2*g*sin(q2))
J2 = m4_val;

fprintf('\nInercias de diseño para modelos lineales SISO:\n');
fprintf('  Eje 1: J1_max = %.2f kg*m^2 -> Funcion de transferencia: Q1(s)/T1(s) = 1 / (%.1f*s^2)\n', J1_max, J1_max);
fprintf('  Eje 2: J2     = %.2f kg*m^2 -> Funcion de transferencia: Q2(s)/T2_pid(s) = 1 / (%.1f*s^2)\n', J2, J2);

% Sintonización PID:
% Planta: G(s) = 1 / (J * s^2)
% PID: C(s) = (Kd*s^2 + Kp*s + Ki)/s
% Lazo cerrado: s^3 + (Kd/J)*s^2 + (Kp/J)*s + (Ki/J) = 0
% Polinomio deseado: (s^2 + 2*xi*wn*s + wn^2)*(s + po) = s^3 + (2*xi*wn + po)*s^2 + (wn^2 + 2*xi*wn*po)*s + po*wn^2

% Especificaciones Eje 1:
wn1 = 3.0; xi1 = 1.0; po1 = 12.0; % po = 4*wn
Kd1 = J1_max * (2*xi1*wn1 + po1);
Kp1 = J1_max * (wn1^2 + 2*xi1*wn1*po1);
Ki1 = J1_max * (po1 * wn1^2);

% Especificaciones Eje 2:
wn2 = 4.0; xi2 = 1.0; po2 = 16.0; % po = 4*wn
Kd2 = J2 * (2*xi2*wn2 + po2);
Kp2 = J2 * (wn2^2 + 2*xi2*wn2*po2);
Ki2 = J2 * (po2 * wn2^2);

fprintf('\nParámetros PID Calculados:\n');
fprintf('  Eje 1 (wn=%.1f, xi=%.1f, po=%.1f): Kp1 = %.2f, Kd1 = %.2f, Ki1 = %.2f\n', wn1, xi1, po1, Kp1, Kd1, Ki1);
fprintf('  Eje 2 (wn=%.1f, xi=%.1f, po=%.1f): Kp2 = %.2f, Kd2 = %.2f, Ki2 = %.2f\n', wn2, xi2, po2, Kp2, Kd2, Ki2);

%% PARTE 2: PARÁMETROS DINÁMICOS NEWTON-EULER DEL ESLABÓN 2 (MANIVELA)
fprintf('\n-----------------------------------------------------------------\n');
fprintf('--- PARTE 2: PARÁMETROS NEWTON-EULER ESLABÓN 2 (MANIVELA) ---\n');
fprintf('-----------------------------------------------------------------\n');

syms L2A L2B R rho real positive
% Geometría: 3 varillas cilíndricas macizas:
% Varilla 1: Tramo inferior en X de longitud L2B/2, centrado en (L2B/4, 0, 0)
% Varilla 2: Tramo vertical en Y de longitud L2A, centrado en (L2B/2, L2A/2, 0)
% Varilla 3: Tramo superior en X de longitud L2B/2, centrado en (3*L2B/4, L2A, 0)

m_tramo1 = rho * (L2B/2);
m_tramo2 = rho * L2A;
m_tramo3 = rho * (L2B/2);
m2_total = m_tramo1 + m_tramo2 + m_tramo3; % rho*(L2A + L2B)

% Centro de masas CoM respecto al origen del sistema {2} (base de la manivela):
x_CoM = (m_tramo1*(L2B/4) + m_tramo2*(L2B/2) + m_tramo3*(3*L2B/4)) / m2_total;
x_CoM = simplify(x_CoM); % Debe ser L2B / 2 por simetría horizontal

y_CoM = (m_tramo1*0 + m_tramo2*(L2A/2) + m_tramo3*L2A) / m2_total;
y_CoM = simplify(y_CoM);

z_CoM = 0;

fprintf('Masa total eslabón 2: m2 = rho*(L2A + L2B)\n');
fprintf('Centro de masas respecto a origen {2}:\n');
fprintf('  xC = %s\n', char(x_CoM));
fprintf('  yC = %s\n', char(y_CoM));
fprintf('  zC = 0\n');

% Vector s22: Posición del CoM del eslabón 2 respecto al sistema {2}:
% Si el sistema {2} tiene origen en la articulación 2 (extremo superior o inferior según convenio):
% Vector del origen {2} al CoM:
s22 = [x_CoM - L2B; y_CoM - L2A; 0];
fprintf('Vector s22 (CoM respecto a origen {2}):\n');
disp(s22);

fprintf('¡Cálculo simbólico y numérico completado con éxito!\n');