%% =========================================================================
%% EXAMEN OFICIAL ROBOTICA - CONVOCATORIA 8 DE JULIO DE 2025 (P3)
%% Grado en Ingenieria de Tecnologias Industriales - E.S.I. Sevilla
%% =========================================================================
% Script analitico, simbolico y numerico para resolver completamente:
% PROBLEMA 1 (2.0 puntos):
%   a) Modelo matricial: M(q), C(q,qd), g(q) y propiedad de antisimetria
%   b) Modelos aproximados lineales de cada articulacion
%   c) Diseno analitico y sintonizacion de control PID
% PROBLEMA 2 (1.5 puntos):
%   Parametros dinamicos Newton-Euler del Eslabon 2 (manivela compuesta):
%   Masa m2, centro de gravedad local s22 y tensor de inercia baricentrico I22
% =========================================================================

clear; clc; close all;

fprintf('=========================================================================\n');
fprintf('  RESOLUCION OFICIAL: EXAMEN GITI JULIO 2025 - PROBLEMA 3 (DINAMICA/CONTROL)\n');
fprintf('=========================================================================\n\n');

%% =========================================================================
%% PROBLEMA 1: MODELADO DINAMICO MATRICIAL Y CONTROL PID
%% =========================================================================
fprintf('--- PROBLEMA 1: MODELO DINAMICO Y CONTROL ---\n\n');

syms q1 q2 qd1 qd2 qdd1 qdd2 real
syms m1 m2 L1 L2 I1 I2 g T1 T2 real positive

% Ecuaciones dadas en el enunciado:
% (m1*L1^2 + I1 + I2 + m2*q2^2)*qdd1 + 2*m2*q2*qd1*qd2 + (m1*L1 + m2*q2)*g*cos(q1) = T1
% m2*qdd2 - m2*q2*qd1^2 + m2*g*sin(q1) = T2

% Apartado a) Matrices dinamicas
M = [ m1*L1^2 + I1 + I2 + m2*q2^2, 0 ;
      0,                            m2 ];

% Matriz C tal que C*[qd1; qd2] = [2*m2*q2*qd1*qd2; -m2*q2*qd1^2]
% Con Christoffel:
C = [ m2*q2*qd2,  m2*q2*qd1 ;
     -m2*q2*qd1,  0 ];

g_vec = [ (m1*L1 + m2*q2)*g*cos(q1) ;
           m2*g*sin(q1) ];

fprintf('a) Modelo Matricial: M(q)*qdd + C(q,qd)*qd + g(q) = T\n');
fprintf('Matriz de Inercia M(q):\n');
disp(M);

fprintf('Matriz de Coriolis C(q,qd):\n');
disp(C);

fprintf('Vector de Gravedad g(q):\n');
disp(g_vec);

% Comprobacion de la propiedad de antisimetria de dot(M) - 2*C:
dM = [ 2*m2*q2*qd2, 0 ; 0, 0 ];
N = simplify(dM - 2*C);
fprintf('Matriz dM - 2C (debe ser antisimetrica N + N^T = 0):\n');
disp(N);
fprintf('Verificacion antisimetria: %s\n\n', mat2str(isequal(simplify(N + N.'), zeros(2,2))));

% Apartado b) Modelos aproximados lineales de cada articulacion
fprintf('b) Modelos Aproximados Lineales Desacoplados:\n');
fprintf('Despreciando el acoplamiento cruzado y considerando la inercia maxima de cada eje:\n');
fprintf('  Articulacion 1: J_1max * qdd1 + B_1 * qd1 = T1 - g1(q)\n');
fprintf('  Articulacion 2: m2 * qdd2 + B_2 * qd2 = T2 - g2(q)\n\n');

% Apartado c) Diseno de control PID con datos numericos
m1_val = 1.0;     % kg
m2_val = 1.5;     % kg
L1_val = 1.5;     % m
L2_val = 2.0;     % m (alcance radial maximo q2_max = L2)
I1_val = 0.25;    % kg m^2
I2_val = 0.15;    % kg m^2
g_val  = 9.81;    % m/s^2

% Inercia maxima de articulacion 1 en q2 = L2:
J1_max = m1_val * L1_val^2 + I1_val + I2_val + m2_val * L2_val^2;
J2_val = m2_val;

fprintf('c) Diseno de Control PID:\n');
fprintf('  Inercia maxima Eje 1 (q2 = L2 = 2.0 m): J_1max = %.4f kg*m^2\n', J1_max);
fprintf('  Inercia equivalente Eje 2: J_2 = %.4f kg\n\n', J2_val);

% Especificaciones de diseno:
% Frecuencia natural deseada wn y amortiguamiento critico xi = 1:
wn1 = 3.0; % rad/s
xi1 = 1.0;
Kp1 = J1_max * wn1^2;
Kd1 = 2 * xi1 * wn1 * J1_max;
Ki1 = Kp1 * (wn1 / 5); % Accion integral para anular droop gravitatorio

wn2 = 4.0; % rad/s
xi2 = 1.0;
Kp2 = J2_val * wn2^2;
Kd2 = 2 * xi2 * wn2 * J2_val;
Ki2 = Kp2 * (wn2 / 5);

fprintf('Parametros sintonizados para Controlador PID Articulacion 1:\n');
fprintf('  Kp1 = %.4f N*m/rad\n', Kp1);
fprintf('  Kd1 = %.4f N*m*s/rad\n', Kd1);
fprintf('  Ki1 = %.4f N*m/(rad*s)\n\n', Ki1);

fprintf('Parametros sintonizados para Controlador PID Articulacion 2:\n');
fprintf('  Kp2 = %.4f N/m\n', Kp2);
fprintf('  Kd2 = %.4f N*s/m\n', Kd2);
fprintf('  Ki2 = %.4f N/(m*s)\n\n', Ki2);

%% =========================================================================
%% PROBLEMA 2: PARAMETROS DINAMICOS NEWTON-EULER DEL ESLABON 2
%% =========================================================================
fprintf('--- PROBLEMA 2: PARAMETROS DINAMICOS NEWTON-EULER DEL ESLABON 2 ---\n\n');

syms L2A_sym L2B_sym R_sym rho_sym real positive

% Varilla 1: horizontal inferior de longitud L2B/2
m1_rod = rho_sym * (L2B_sym / 2);
% Varilla 2: vertical intermedia de longitud L2A
m2_rod = rho_sym * L2A_sym;
% Varilla 3: horizontal superior de longitud L2B/2
m3_rod = rho_sym * (L2B_sym / 2);

% 1. Masa total del eslabon 2:
m2_tot = simplify(m1_rod + m2_rod + m3_rod);
fprintf('1. Masa total del eslabon 2 (m2):\n');
fprintf('   m2 = %s = rho * (L2A + L2B)\n\n', char(m2_tot));

% 2. Centro de gravedad relativo:
% Referencia tomada en el inicio (Articulacion 2):
x_c1 = L2B_sym / 4;        y_c1 = sym(0);
x_c2 = L2B_sym / 2;        y_c2 = L2A_sym / 2;
x_c3 = 3 * L2B_sym / 4;    y_c3 = L2A_sym;

xc_rel = simplify((m1_rod*x_c1 + m2_rod*x_c2 + m3_rod*x_c3) / m2_tot);
yc_rel = simplify((m1_rod*y_c1 + m2_rod*y_c2 + m3_rod*y_c3) / m2_tot);

fprintf('2. Coordenadas del CdM respecto a Articulacion 2:\n');
fprintf('   xc = %s\n', char(xc_rel));
fprintf('   yc = %s\n\n', char(yc_rel));

% Vector s22: desde el origen de {S2} (Articulacion 3 en (L2B, L2A)) al CdM:
s22_sym = [ xc_rel - L2B_sym ;
            yc_rel - L2A_sym ;
            sym(0) ];

fprintf('Vector s22 en el marco {S2} (origen O2 en Articulacion 3):\n');
disp(s22_sym);

% 3. Tensor de Inercia Baricentrico I22 (Teorema de Steiner)
dx1 = x_c1 - xc_rel;  dy1 = y_c1 - yc_rel;
dx2 = x_c2 - xc_rel;  dy2 = y_c2 - yc_rel;
dx3 = x_c3 - xc_rel;  dy3 = y_c3 - yc_rel;

% Inercias elementales canónicas:
% 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_rod*R_sym^2 + m1_rod*dy1^2;
Iyy_1 = (1/12)*m1_rod*(L2B_sym/2)^2 + m1_rod*dx1^2;
Izz_1 = (1/12)*m1_rod*(L2B_sym/2)^2 + m1_rod*(dx1^2 + dy1^2);

% 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_rod*L2A_sym^2 + m2_rod*dy2^2;
Iyy_2 = (1/2)*m2_rod*R_sym^2 + m2_rod*dx2^2;
Izz_2 = (1/12)*m2_rod*L2A_sym^2 + m2_rod*(dx2^2 + dy2^2);

% Varilla 3 (horizontal en X):
Ixx_3 = (1/2)*m3_rod*R_sym^2 + m3_rod*dy3^2;
Iyy_3 = (1/12)*m3_rod*(L2B_sym/2)^2 + m3_rod*dx3^2;
Izz_3 = (1/12)*m3_rod*(L2B_sym/2)^2 + m3_rod*(dx3^2 + dy3^2);

% Suma total baricéntrica:
Ixx_bar = simplify(Ixx_1 + Ixx_2 + Ixx_3);
Iyy_bar = simplify(Iyy_1 + Iyy_2 + Iyy_3);
Izz_bar = simplify(Izz_1 + Izz_2 + Izz_3);
Ixy_bar = simplify(- (m1_rod*dx1*dy1 + m2_rod*dx2*dy2 + m3_rod*dx3*dy3));

I22_sym = [ Ixx_bar,  Ixy_bar,  0 ;
            Ixy_bar,  Iyy_bar,  0 ;
            0,        0,        Izz_bar ];

fprintf('Tensor de Inercia Baricentrico I22 (simbolico):\n');
disp(I22_sym);

% Evaluacion numerica de ejemplo (R = 0.05 m, rho = 1.5 kg/m, L2A = 0.4 m, L2B = 0.8 m):
R_num = 0.05;
rho_num = 1.5;
L2A_num = 0.4;
L2B_num = 0.8;

m2_num = double(subs(m2_tot, [R_sym, rho_sym, L2A_sym, L2B_sym], [R_num, rho_num, L2A_num, L2B_num]));
s22_num = double(subs(s22_sym, [R_sym, rho_sym, L2A_sym, L2B_sym], [R_num, rho_num, L2A_num, L2B_num]));
I22_num = double(subs(I22_sym, [R_sym, rho_sym, L2A_sym, L2B_sym], [R_num, rho_num, L2A_num, L2B_num]));

fprintf('Evaluacion Numerica para R=0.05m, rho=1.5 kg/m, L2A=0.4m, L2B=0.8m:\n');
fprintf('  m2 = %.4f kg\n', m2_num);
fprintf('  s22 = [%.4f; %.4f; %.4f] m\n', s22_num(1), s22_num(2), s22_num(3));
fprintf('  I22 =\n');
disp(I22_num);

fprintf('=========================================================================\n');
fprintf('  RESOLUCION EXAMEN JULIO 2025 P3 COMPLETADA EXITOSAMENTE\n');
fprintf('=========================================================================\n');