%% cinemáticas (sub)

%% cinemática completa

clc, clear all , close all

zz_angular_displacement = 'movezz_angular_displacement.csv';
convert_fact = 0.061035156; %Converte ADC em º/s

%humanoid_state
humanoid_data_zz;


L1=45;
L2=132;
L3=123;
L4 = 50;
seixos;

DHRR = [pi/2 0 L1 pi/2
        pi/2 0 0 pi/2
        0 L2 0 pi
        0 L3 0 pi
        0 0 0 -pi/2
        0 L4 0 0];
        
trr=[0 0 0 0 0 0]';

MDHcima = [];
for n = 1:size(humanoid_state,1)
    MDHcima(:,:,n) =[pi/2 0 L1 pi/2
        deg2rad(humanoid_state(n,1))+pi/2 0 0 pi/2
        deg2rad(humanoid_state(n,3)) L2 0 0
        deg2rad(humanoid_state(n,5)) L3 0 0
        deg2rad(humanoid_state(n,9)) 0 0 -pi/2
        deg2rad(humanoid_state(n,7)) L4 0 0];
end
AArr=Tlinks(DHRR);

Orgrr=LinkOrigins(AArr);
axis([-300 300 -300 300 -100 400])
hmanip=DrawLinks(Orgrr);

AAAcima=CalculateRobotMotion(MDHcima);

Hcima=DrawFrames(AAAcima(:,:,:,1),P);

AnimateRobot(Hcima,AAAcima,P,hmanip,0.1);

%% sub-cinemática - sensores canela (6 ou 8)

clc, clear all , close all

zz_angular_displacement = 'movezz_angular_displacement.csv';
convert_fact = 0.061035156; %Converte ADC em º/s

humanoid_state_zz = 'movezz_humanoid_state.csv';

% humanoid state variables
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
init_time = data(1,3);

L1=45;
L2=22;
% L3=123;

seixos;

DHRR = [pi/2 0 L1 pi/2
        pi/2 0 0 pi/2
        0 L2 0 0];
        
trr=[0 0 0 ]';

MDHcima = [];
for n = 1:size(humanoid_state,1)
    MDHcima(:,:,n) =[pi/2 0 L1 pi/2
        deg2rad(humanoid_state(n,1))+pi/2 0 0 pi/2
        deg2rad(humanoid_state(n,3)) L2 0 0];
end
AArr=Tlinks(DHRR);

Orgrr=LinkOrigins(AArr);
axis([-300 300 -300 300 -100 400])
hmanip=DrawLinks(Orgrr);

AAAcima=CalculateRobotMotion(MDHcima);

Hcima=DrawFrames(AAAcima(:,:,:,1),P);

AnimateRobot(Hcima,AAAcima,P,hmanip,0.1);

%% sub-cinemática para a coxa (sensores 5 e 7)

clc, clear all , close all

zz_angular_displacement = 'movezz_angular_displacement.csv';
convert_fact = 0.061035156; %Converte ADC em º/s

humanoid_state_zz = 'movezz_humanoid_state.csv';

% humanoid state variables
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
init_time = data(1,3);

L1=45;
L2=132;
L3=42;

seixos;

DHRR = [pi/2 0 L1 pi/2
        pi/2 0 0 pi/2
        0 L2 0 0
        0 L3 0 0];
        
trr=[0 0 0 0]';

MDHcima = [];
for n = 1:size(humanoid_state,1)
    MDHcima(:,:,n) =[pi/2 0 L1 pi/2
        deg2rad(humanoid_state(n,1))+pi/2 0 0 pi/2
        deg2rad(humanoid_state(n,3)) L2 0 0
        deg2rad(humanoid_state(n,5)) L3 0 0
        deg2rad(humanoid_state(n,9)) 0 0 -pi/2
        deg2rad(humanoid_state(n,7)) 0 0 0];
end
AArr=Tlinks(DHRR);

Orgrr=LinkOrigins(AArr);
axis([-300 300 -300 300 -100 400])
hmanip=DrawLinks(Orgrr);

AAAcima=CalculateRobotMotion(MDHcima);

Hcima=DrawFrames(AAAcima(:,:,:,1),P);

AnimateRobot(Hcima,AAAcima,P,hmanip,0.1);

