%% =========================================================================
% EXAMEN ROBÓTICA GITI - PRIMERA CONVOCATORIA (5 JUNIO 2018) - PROBLEMA 2
% =========================================================================
% Robot Plano 2 GDL de Accionamiento Directo con Articulaciones R-P
% 1. Modelo Dinámico Directo con Intensidades de Corriente (Kti * Ii)
% 2. Programación de la función ModeloDinamico_R.m
% 3. Implementación y Sintonización de Controlador PID
% =========================================================================

clear; clc; close all;
fprintf('=================================================================\n');
fprintf('  EXAMEN ROBÓTICA GITI - JUNIO 2018 - PROBLEMA 2 (ROBOT RP DIRECT-DRIVE)\n');
fprintf('=================================================================\n\n');

%% PARTE 1: PARÁMETROS FÍSICOS OFICIALES DEL ENUNCIADO
M_A = 1.0;          % Masa de la base cilíndrica A [kg]
I_A33 = 0.02;       % Inercia propia de A respecto al eje vertical Z [kg*m^2]

M_B = 1.0;          % Masa de la barra prismática B [kg]
I_B33 = 0.5;        % Inercia baricéntrica de B respecto al eje vertical Z [kg*m^2]
L = 0.5;            % Semilongitud de la barra (longitud total 2L = 1 m)

% Motores de accionamiento directo:
J_m1 = 0.025;       % Inercia del rotor motor 1 [kg*m^2]
J_m2 = 0.025;       % Inercia del rotor motor 2 [kg] (o equivalente lineal)
B_m1 = 2e-5;        % Coeficiente de rozamiento viscoso eje 1 [Nm/(rad/s)]
B_m2 = 2e-5;        % Coeficiente de rozamiento viscoso eje 2 [N/(m/s)]
K_t1 = 10.0;        % Constante de par del motor 1 [Nm/A]
K_t2 = 10.0;        % Constante de fuerza del motor 2 [N/A]

% Inercia base fija respecto al eje de giro:
J0 = I_A33 + I_B33 + J_m1; % 0.02 + 0.5 + 0.025 = 0.545 kg*m^2
M2_eff = M_B + J_m2;       % 1.0 + 0.025 = 1.025 kg

fprintf('Parámetros Globales Calculados:\n');
fprintf('  Inercia base propia J0 = %.3f kg*m^2\n', J0);
fprintf('  Masa efectiva eje 2 M2 = %.3f kg\n', M2_eff);
fprintf('  Constantes de motor: Kt1 = %.1f Nm/A, Kt2 = %.1f N/A\n', K_t1, K_t2);

%% PARTE 2: ECUACIONES DINÁMICAS MATRICIALES
% Vector de variables articulares: q = [q1; q2] (q1: giro rad, q2: traslación m)
% Matriz de inercia M(q):
%   M11(q2) = J0 + M_B * q2^2
%   M12 = M21 = 0 (ortogonales en el plano)
%   M22 = M2_eff
% Vector de fuerzas centrífugas y de Coriolis:
%   V1(q, qd) = 2 * M_B * q2 * qd1 * qd2  (término de Coriolis)
%   V2(q, qd) = - M_B * q2 * qd1^2        (término centrífugo)
% Vector gravitatorio: G(q) = [0; 0] (movimiento en plano horizontal, g ortogonal)

fprintf('\nEcuaciones Dinámicas del Robot RP:\n');
fprintf('  (J0 + M_B*q2^2)*qdd1 + 2*M_B*q2*qd1*qd2 + B_m1*qd1 = Kt1 * I1\n');
fprintf('  M2_eff*qdd2 - M_B*q2*qd1^2 + B_m2*qd2 = Kt2 * I2\n');

%% PARTE 3: SINTONIZACIÓN ANALÍTICA DE CONTROLADORES PID
% Modelo lineal aproximado (SISO desacoplado):
% Eje 1: Peor caso de inercia con máxima extensión q2 = L = 0.5 m:
J1_max = J0 + M_B * (L^2); % 0.545 + 1.0*(0.25) = 0.795 kg*m^2
% Función de transferencia Eje 1: Q1(s)/I1(s) = Kt1 / (J1_max * s^2)

% Eje 2: Inercia constante:
% Función de transferencia Eje 2: Q2(s)/I2(s) = Kt2 / (M2_eff * s^2)

% Especificaciones de lazo cerrado (amortiguamiento crítico xi=1, polo rápido po = 4*wn):
% s^3 + (Kd*Kt/J)*s^2 + (Kp*Kt/J)*s + (Ki*Kt/J) = (s + wn)^2 * (s + 4*wn)
% -> Kd = (J / Kt) * 6*wn
% -> Kp = (J / Kt) * 9*wn^2
% -> Ki = (J / Kt) * 4*wn^3

wn1 = 4.0; % rad/s
Kd1 = (J1_max / K_t1) * (6 * wn1);
Kp1 = (J1_max / K_t1) * (9 * wn1^2);
Ki1 = (J1_max / K_t1) * (4 * wn1^3);

wn2 = 5.0; % rad/s
Kd2 = (M2_eff / K_t2) * (6 * wn2);
Kp2 = (M2_eff / K_t2) * (9 * wn2^2);
Ki2 = (M2_eff / K_t2) * (4 * wn2^3);

fprintf('\nGanancias del Controlador PID para las Intensidades [A/rad] y [A/m]:\n');
fprintf('  Eje 1 (wn = %.1f rad/s): Kp1 = %.3f, Kd1 = %.3f, Ki1 = %.3f\n', wn1, Kp1, Kd1, Ki1);
fprintf('  Eje 2 (wn = %.1f rad/s): Kp2 = %.3f, Kd2 = %.3f, Ki2 = %.3f\n', wn2, Kp2, Kd2, Ki2);

%% PARTE 4: SIMULACIÓN TEMPORAL DE RESPUESTA ANTE ESCALÓN
tspan = 0:0.005:3.0;
q_ref = [deg2rad(45); 0.3]; % Consigna: 45 grados y 0.3 metros

% Simulación mediante integración de Euler:
N = length(tspan);
q = zeros(2, N);
qd = zeros(2, N);
I_control = zeros(2, N);
e_int = [0; 0];

dt = tspan(2) - tspan(1);
for k = 1:N-1
    % Error de posición y velocidad:
    e = q_ref - q(:, k);
    ed = - qd(:, k);
    e_int = e_int + e * dt;
    
    % Ley de control PID para corrientes:
    i1 = Kp1 * e(1) + Kd1 * ed(1) + Ki1 * e_int(1);
    i2 = Kp2 * e(2) + Kd2 * ed(2) + Ki2 * e_int(2);
    I_control(:, k) = [i1; i2];
    
    % Dinámica directa no lineal (despeje de aceleraciones):
    q2_current = q(2, k);
    qd1_c = qd(1, k);
    qd2_c = qd(2, k);
    
    qdd1 = (K_t1 * i1 - 2 * M_B * q2_current * qd1_c * qd2_c - B_m1 * qd1_c) / (J0 + M_B * q2_current^2);
    qdd2 = (K_t2 * i2 + M_B * q2_current * qd1_c^2 - B_m2 * qd2_c) / M2_eff;
    
    % Integración de estados:
    qd(:, k+1) = qd(:, k) + [qdd1; qdd2] * dt;
    q(:, k+1)  = q(:, k)  + qd(:, k+1) * dt;
end
I_control(:, N) = I_control(:, N-1);

% Graficación de resultados:
figure('Color', 'w', 'Position', [100 100 850 450]);
subplot(2, 1, 1);
plot(tspan, rad2deg(q(1, :)), 'b-', 'LineWidth', 2); hold on;
plot(tspan, rad2deg(q_ref(1))*ones(size(tspan)), 'r--', 'LineWidth', 1.5);
grid on; ylabel('q_1 (grados)'); title('Respuesta en Lazo Cerrado con PID - Robot RP');
legend('Respuesta real', 'Referencia 45°', 'Location', 'southeast');

subplot(2, 1, 2);
plot(tspan, q(2, :), 'g-', 'LineWidth', 2); hold on;
plot(tspan, q_ref(2)*ones(size(tspan)), 'r--', 'LineWidth', 1.5);
grid on; xlabel('Tiempo (s)'); ylabel('q_2 (m)');
legend('Respuesta real', 'Referencia 0.3 m', 'Location', 'southeast');

saveas(gcf, 'Respuesta_Simulacion_PID_Junio_2018_P2.png');
fprintf('Gráfica Respuesta_Simulacion_PID_Junio_2018_P2.png guardada con éxito!\n');