%% Control PD robot arm with elastic joints (Step)
clc;
clear;
close all;
load('ABCD');

sim('robot_arm_elastic_PD_lado_motor_ref_pos_step.slx')

% Posición eslabón 1
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q1);
plot(time,theta1), title('Respuesta ante escalón (Posición eslabón 1)')
plot(time,stepref);
grid, legend('q1','theta1','qref1','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad)');

% Posición eslabón 2
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q2);
plot(time,theta2), title('Respuesta ante escalón (Posición eslabón 2)')
plot(time,stepref);
grid, legend('q2','theta2','qref2','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad)');

% Velocidad eslabón 1
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q_prim1);
plot(time,theta_prim1), title('Respuesta ante escalón (Velocidad eslabón 1)')
grid, legend('q-prim1','theta-prim1','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad/s)');

% Velocidad eslabón 2
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q_prim2);
plot(time,theta_prim2), title('Respuesta ante escalón (Velocidad eslabón 2)')
grid, legend('q-prim2','theta-prim2','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad/s)');


% Acción de control
figure;
set(gca,'fontsize',12);
hold on;
plot(time,u), title('Acción de control'), legend('u1','u2','Location','southeast','Orientation','vertical');
grid;
xlabel('Tiempo (s)');
ylabel('Par (Nm)');

figure, set(gca,'fontsize',12), hold on, plot(q1,q_prim1), grid, title('Velocidad1/Posición1');
xlabel('Posición (rad)');
ylabel('Velocidad (rad/s)');
figure, set(gca,'fontsize',12), hold on,  plot(q2,q_prim2), grid, title('Velocidad2/Posición2');
xlabel('Posición (rad)');
ylabel('Velocidad (rad/s)');


%% Control PD robot arm with elastic joints (Trajectory follow)
clc;
clear;
%close all;
load('ABCD_0.mat');

tm = 0.01;
[qref3 vref3 aref3 jerk jerk_prim t3] = trapez4( 0, 1.75, 0.5, 2, 10, 500, tm,1.01); %trapez4( 0, 1, 0.5, 3, 20, 800, tm,1.2);
[qref4 vref4 aref4 jerk4 jerk_prim4 t4] = trapez4( 0, 0.5, 0.5, 2, 10, 500, tm,3.51);
% plot(qref3)
% hold
% plot(qref4)

Kp1=1500; %2000;
Kv1=50; % 100;
Kp2=2500; %2000;
Kv2=50; %100;
sim('robot_arm_elastic_PD_lado_motor_ref_pos_pos.slx')

ecuad(qref3,q1(1:300,1)) % Error cuadrático medio posición eslbón 1.  
ecuad(vref3,q_prim1(1:300,1)) % Error cuadrático medio velocidad eslabón 1.

ecuad(qref4,q2(1:300,1)) % Error cuadrático medio posición eslbón 2.  
ecuad(vref4,q_prim2(1:300,1)) % Error cuadrático medio velocidad eslabón 2.

% Posición eslabón 1
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q1);
plot(time,theta1), title('Seguimiento de trayectoria (Posición eslabón 1)')
plot(t3,qref3);
grid, legend('q1','theta1','qref1','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad)');

% Posición eslabón 2
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q2);
plot(time,theta2), title('Seguimiento de trayectoria (Posición eslabón 2)')
plot(t4,qref4);
grid, legend('q2','theta2','qref2','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad)');

% Velocidad eslabón 1
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q_prim1);
plot(time,theta_prim1), title('Seguimiento de trayectoria (Velocidad eslabón 1)')
plot(t3,vref3);
grid, legend('q-prim1','theta-prim1','vref1','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad/s)');

% Velocidad eslabón 2
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q_prim2);
plot(time,theta_prim2), title('Seguimiento de trayectoria (Velocidad eslabón 2)')
plot(t4,vref4);
grid, legend('q-prim2','theta-prim2','vref2','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad/s)');


% Acción de control
figure;
set(gca,'fontsize',12);
hold on;
plot(time,u), title('Acción de control'), legend('u1','u2','Location','southeast','Orientation','vertical');
grid;
xlabel('Tiempo (s)');
ylabel('Par (Nm)');

figure, set(gca,'fontsize',12), hold on, plot(q1,q_prim1), grid, title('Velocidad1/Posición1');
xlabel('Posición (rad)');
ylabel('Velocidad (rad/s)');
figure, set(gca,'fontsize',12), hold on,  plot(q2,q_prim2), grid, title('Velocidad2/Posición2');
xlabel('Posición (rad)');
ylabel('Velocidad (rad/s)');

%% Control PD robot arm with elastic joints (Trajectory follow + GRAVITY)
clc;
clear;
close all;
load('ABCD.mat');

tm = 0.01;
[qref3 vref3 aref3 jerk jerk_prim t3] = trapez4( 0, 0.75, 0.5, 2, 10, 500, tm,1.01); %trapez4( 0, 1, 0.5, 3, 20, 800, tm,1.2);
[qref4 vref4 aref4 jerk4 jerk_prim4 t4] = trapez4( 0, 0.5, 0.5, 2, 10, 500, tm,1.51);
% plot(qref3)
% hold
% plot(qref4)

Kp1=2500; %2000;
Kv1=50; % 100;
Kp2=2500; %2000;
Kv2=50; %100;
sim('robot_arm_elastic_PD_lado_motor_ref_pos_gravity.slx')

ecuad(qref3,q1(1:300,1)) % Error cuadrático medio posición eslbón 1.  
ecuad(vref3,q_prim1(1:300,1)) % Error cuadrático medio velocidad eslabón 1.

ecuad(qref4,q2(1:300,1)) % Error cuadrático medio posición eslbón 2.  
ecuad(vref4,q_prim2(1:300,1)) % Error cuadrático medio velocidad eslabón 2.

% Posición eslabón 1
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q1);
plot(time,theta1), title('Seguimiento de trayectoria (Posición eslabón 1)')
plot(t3,qref3);
grid, legend('q1','theta1','qref1','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad)');

% Posición eslabón 2
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q2);
plot(time,theta2), title('Seguimiento de trayectoria (Posición eslabón 2)')
plot(t4,qref4);
grid, legend('q2','theta2','qref2','Location','southeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad)');

% Velocidad eslabón 1
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q_prim1);
plot(time,theta_prim1), title('Seguimiento de trayectoria (Velocidad eslabón 1)')
plot(t3,vref3);
grid, legend('q-prim1','theta-prim1','vref1','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad/s)');

% Velocidad eslabón 2
figure;
set(gca,'fontsize',12);
hold on;
plot(time,q_prim2);
plot(time,theta_prim2), title('Seguimiento de trayectoria (Velocidad eslabón 2)')
plot(t4,vref4);
grid, legend('q-prim2','theta-prim2','vref2','Location','northeast','Orientation','vertical');
xlabel('Tiempo (s)');
ylabel('Posición (rad/s)');


% Acción de control
figure;
set(gca,'fontsize',12);
hold on;
plot(time,u), title('Acción de control'), legend('u1','u2','Location','southeast','Orientation','vertical');
grid;
xlabel('Tiempo (s)');
ylabel('Par (Nm)');

figure, set(gca,'fontsize',12), hold on, plot(q1,q_prim1), grid, title('Velocidad1/Posición1');
xlabel('Posición (rad)');
ylabel('Velocidad (rad/s)');
figure, set(gca,'fontsize',12), hold on,  plot(q2,q_prim2), grid, title('Velocidad2/Posición2');
xlabel('Posición (rad)');
ylabel('Velocidad (rad/s)');

