%% Experiência 6

J1x_y=[0 0];
J1x_x=[0 12.28];

J2x_y=[0 0 -90 0 0];
J2x_x=[0 2.6 4.06 5.6 12.28];

plot(J1x_x,J1x_y,log_tempo(1,1:end-1),log_pos_s0(:,1)*180/pi,J2x_x,J2x_y,log_tempo(1,1:end-1),log_pos_s1(:,1)*180/pi)
grid on;
legend('J3','O1_s_x','J5','O2_s_x')
xlabel('Tempo(s)');ylabel('Posição angular(º)')

%% Experiência 7

J1x_y=[0 0 80 0 0];
J1x_x=[0 3.5 5.5 7.4 15.28];

J2x_y=[0 0 -90 -90 0 0];
J2x_x=[0 1.95 3.5 5.5 7.4 15.28];

plot(J1x_x,J1x_y,log_tempo(1,1:end-1),log_pos_s0(:,1)*180/pi,J2x_x,J2x_y,log_tempo(1,1:end-1),log_pos_s1(:,1)*180/pi)
grid on;
legend('J3','O1_s_x','J5','O2_s_x')
xlabel('Tempo(s)');ylabel('Posição angular(º)')

%% Experiência 8

J1x_y=[0 0 90 90 0 0];
J1x_x=[0 1.8 4.1 6 8.1 19.3];

J1y_y=[0 0 -90 -90];
J1y_x=[0 4.1 6 19.3];

plot(J1x_x,J1x_y,'k',log_tempo(1,1:end-1),log_pos_s0(:,1)*180/pi,'k--',J1y_x,J1y_y,'b',log_tempo(1,1:end-1),log_pos_s0(:,2)*180/pi,'b--')
grid on;
legend('J3','O1_s_x','J1','O1_s_y')
xlabel('Tempo(s)');ylabel('Posição angular(º)')

%% Experiência 1 dia 23 de Maio

plot(log_tempo(1,1:end-1),ang_euler.fanuc.fi,log_tempo(1,1:end-1),ang_euler.fanuc.teta,log_tempo(1,1:end-1),ang_euler.fanuc.psi,log_tempo(1,1:end-1),ang_euler.gyro.fi,log_tempo(1,1:end-1),ang_euler.gyro.teta,log_tempo(1,1:end-1),ang_euler.gyro.psi)
grid on;
legend('\Phi','\Theta','\Psi','\Phi_s','\Theta_s','\Psi_s')
xlabel('Tempo(s)');ylabel('Posição angular(º)')

%% Experiência 2 dia 23 de Maio

for i=1:size(ang_euler.gyro.fi,2)
    
    if ang_euler.gyro.fi(1,i)>0 && log_tempo(1,i)>2
        ang_euler.gyro.fi(1,i)=ang_euler.gyro.fi(1,i)-179.6*2;
    end
    
end

plot(log_tempo(1,1:end-1),ang_euler.fanuc.fi,'k',log_tempo(1,1:end-1),ang_euler.fanuc.teta,'b',log_tempo(1,1:end-1),ang_euler.fanuc.psi,'r',log_tempo(1,1:end-1),ang_euler.gyro.fi,'k--',log_tempo(1,1:end-1),ang_euler.gyro.teta,'b--',log_tempo(1,1:end-1),ang_euler.gyro.psi,'r--')
grid on;
legend('\Phi','\Theta','\Psi','\Phi_s','\Theta_s','\Psi_s')
xlabel('Tempo(s)');ylabel('Posição angular(º)')

%% Experiência 1 dia 21 de Junho

J1x_y=[0 0       30       60       30    0       0];
J1x_x=[0 2.08    2.88     3.66     4.44  5.26    10];

J2x_y=[0 0       30       0       -30   0       0];
J2x_x=[0 2.08    2.88     3.66     4.44  5.26    10];

plot(J1x_x,J1x_y,log_tempo(1,1:end-1),log_pos_s0(:,1)*180/pi,J2x_x,J2x_y,log_tempo(1,1:end-1),log_pos_s1(:,1)*180/pi)
grid on;
legend('J3','O1_s_x','J5','O2_s_x')
xlabel('Tempo(s)');ylabel('Posição angular(º)')

%% Experiência 2 dia 21 de Junho

J1x_y=[0    0       -20      20       -20    0       0];
J1x_x=[0    2.25    3.03     4.09     5.12   5.91    12.56];

J2x_y=[0    0       30       -30      30     0       0];
J2x_x=[0    2.25    3.03     4.09     5.12   5.91    12.56];

plot(J1x_x,J1x_y,'k',log_tempo(1,1:end-1),log_pos_s0(:,1)*180/pi,'k--',J2x_x,J2x_y,'b',log_tempo(1,1:end-1),log_pos_s1(:,1)*180/pi,'b--')
grid on;
legend('J3','O1_s_x','J5','O2_s_x')
xlabel('Tempo(s)');ylabel('Posição angular(º)')

%% Experiência 3 dia 21 de Junho

J1x_y=[0 0       30       60       30    0       0];
J1x_x=[0 2.08    2.88     3.66     4.44  5.26    10];

J2x_y=[0 0       30       0       -30   0       0];
J2x_x=[0 2.08    2.88     3.66     4.44  5.26    10];

plot(J1x_x,J1x_y,log_tempo(1,1:end-1),log_pos_s0(:,1)*180/pi,J2x_x,J2x_y,log_tempo(1,1:end-1),log_pos_s1(:,1)*180/pi)
grid on;
legend('J3','O1_s_x','J5','O2_s_x')
xlabel('Tempo(s)');ylabel('Posição angular(º)')