%% servomotors

clear all, close all, clc

save_id = 0;

x_label = 'Segundos [sec]';
y_label = 'Posição angular - graus [º]';


humanoid_state_zz = 'movezz_humanoid_state.csv';
humanoid_state_xx = 'movexx_humanoid_state.csv';
humanoid_state_yy = 'moveyy_humanoid_state.csv';

% MOVEZ_ZZ
importfile(humanoid_state_zz);
humanoid_state(:,1) = data(:,19); % servo 11
humanoid_state(:,2) = data(:,20); % servo 21
humanoid_state(:,3) = data(:,21); % servo 12
humanoid_state(:,4) = data(:,22); % servo 22
humanoid_state(:,5) = data(:,23); % servo 13
humanoid_state(:,6) = data(:,24); % servo 23
humanoid_state(:,7) = data(:,25); % servo 15
humanoid_state(:,8) = data(:,26); % servo 25
humanoid_state(:,9) = data(:,27); % servo 16
humanoid_state(:,10) = data(:,28); % servo 26
humanoid_state(:,11) = data(:,29); % servo 31
humanoid_state(:,12) = data(:,30); % servo 32
humanoid_state(:,13) = data(:,31); % servo (rotacao_tronco)
humanoid_state(:,14) = data(:,3); % TimeStamp
humanoid_time = (humanoid_state(:,14) - humanoid_state(1,14))*1e-9;
% anulamento de campos com erro de comunicacao
x = find(humanoid_state==65530);
for q = 1:size(x)
    humanoid_state(x(q))=humanoid_state(x(q)-1);
end

figure
hold on , grid on
title('Agachamento.','FontWeight','bold')
plot(humanoid_time,humanoid_state(:,1),'r','LineWidth',2)
plot(humanoid_time,humanoid_state(:,3),'g','LineWidth',2)
plot(humanoid_time,humanoid_state(:,5),'b','LineWidth',2)
plot(humanoid_time,humanoid_state(:,7),'k','LineWidth',2)
plot(humanoid_time,humanoid_state(:,9),'c','LineWidth',2)
legend('servo 11','servo 12','servo 13','servo 15','servo 16','Location','Best')
xlabel(x_label,'FontSize',14)
ylabel(y_label,'FontSize',14)
set(gca,'FontSize',14)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/move_ZZ','jpg')
end

humanoid_state = [];
% MOVEZ_XX
importfile(humanoid_state_xx);
humanoid_state(:,1) = data(:,19); % servo 11
humanoid_state(:,2) = data(:,20); % servo 21
humanoid_state(:,3) = data(:,21); % servo 12
humanoid_state(:,4) = data(:,22); % servo 22
humanoid_state(:,5) = data(:,23); % servo 13
humanoid_state(:,6) = data(:,24); % servo 23
humanoid_state(:,7) = data(:,25); % servo 15
humanoid_state(:,8) = data(:,26); % servo 25
humanoid_state(:,9) = data(:,27); % servo 16
humanoid_state(:,10) = data(:,28); % servo 26
humanoid_state(:,11) = data(:,29); % servo 31
humanoid_state(:,12) = data(:,30); % servo 32
humanoid_state(:,13) = data(:,31); % servo (rotacao_tronco)
humanoid_state(:,14) = data(:,3); % TimeStamp
humanoid_time = (humanoid_state(:,14) - humanoid_state(1,14))*1e-9;
% anulamento de campos com erro de comunicacao
x = find(humanoid_state>=1000);
for q = 1:size(x)
    humanoid_state(x(q))=humanoid_state(x(q)-1);
end

figure
hold on , grid on
title('Flexão joelhos.','FontWeight','bold')
plot(humanoid_time,humanoid_state(:,1),'r','LineWidth',2)
plot(humanoid_time,humanoid_state(:,3),'g','LineWidth',2)
plot(humanoid_time,humanoid_state(:,5),'b','LineWidth',2)
plot(humanoid_time,humanoid_state(:,7),'k','LineWidth',2)
plot(humanoid_time,humanoid_state(:,9),'c','LineWidth',2)
legend('servo 11','servo 12','servo 13','servo 15','servo 16','Location','NorthWest')
xlabel(x_label,'FontSize',14)
ylabel(y_label,'FontSize',14)
set(gca,'FontSize',14)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/move_XX','jpg')
end

humanoid_state = [];
% MOVEZ_XX
importfile(humanoid_state_yy);
humanoid_state(:,1) = data(:,19); % servo 11
humanoid_state(:,2) = data(:,20); % servo 21
humanoid_state(:,3) = data(:,21); % servo 12
humanoid_state(:,4) = data(:,22); % servo 22
humanoid_state(:,5) = data(:,23); % servo 13
humanoid_state(:,6) = data(:,24); % servo 23
humanoid_state(:,7) = data(:,25); % servo 15
humanoid_state(:,8) = data(:,26); % servo 25
humanoid_state(:,9) = data(:,27); % servo 16
humanoid_state(:,10) = data(:,28); % servo 26
humanoid_state(:,11) = data(:,29); % servo 31
humanoid_state(:,12) = data(:,30); % servo 32
humanoid_state(:,13) = data(:,31); % servo (rotacao_tronco)
humanoid_state(:,14) = data(:,3); % TimeStamp
humanoid_time = (humanoid_state(:,14) - humanoid_state(1,14))*1e-9;
% anulamento de campos com erro de comunicacao
x = find(humanoid_state>=1000);
for q = 1:size(x)
    humanoid_state(x(q))=humanoid_state(x(q)-1);
end

figure
hold on , grid on
title('Movimento de anca.','FontWeight','bold')
plot(humanoid_time,humanoid_state(:,1),'r','LineWidth',2)
plot(humanoid_time,humanoid_state(:,3),'g','LineWidth',2)
plot(humanoid_time,humanoid_state(:,5),'b','LineWidth',2)
plot(humanoid_time,humanoid_state(:,7),'k','LineWidth',2)
plot(humanoid_time,humanoid_state(:,9),'c','LineWidth',2)
legend('servo 11','servo 12','servo 13','servo 15','servo 16','Location','Best')
xlabel(x_label,'FontSize',14)
ylabel(y_label,'FontSize',14)
set(gca,'FontSize',14)
if save_id == 1
    saveas(gca,'/home/telmo/Dropbox/Dissertacao/tese_escrita2/Imagens/final_experiments/move_YY','jpg')
end

