%% Splitting the data...


%% Filtered Data!!!!

% MoveZZ

clc ,close all , clear all

zz_angular_displacement = 'movezz_angular_displacement.csv';
humanoid_state_zz = 'movezz_humanoid_state.csv';

sensors_data = [];
humanoid_state = [];
init_time_sensors = [];
init_time_humanoid = [];
humanoid_time = [];
sensors_time = [];
delay_time = [];
% obtencao dos dados dos servomotores -
% 11,21,12,22,13,23,15,25,16,26,31,32,tronco,TimeStamp
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

% obtencao dos dados dos sensors
% Ax,Ay,Az,Gx,Gy,Gz,displacementX,displacementY,displacementZ,Mx,My,Mz,TimeStamp
importfile(zz_angular_displacement);
total_number = data(1,21);
for q = 1:total_number
    sensors_data(:,1,q)=data(:,8+((q-1)*18)); % S_Ax
    sensors_data(:,2,q)=data(:,9+((q-1)*18)); % S_Ay
    sensors_data(:,3,q)=data(:,10+((q-1)*18)); % S_Az
    sensors_data(:,4,q)=data(:,11+((q-1)*18)); % S_Gx
    sensors_data(:,5,q)=data(:,12+((q-1)*18)); % S_Gy
    sensors_data(:,6,q)=data(:,13+((q-1)*18)); % S_Gz
    sensors_data(:,7,q)=data(:,14+((q-1)*18)); % S_displacement_x
    sensors_data(:,8,q)=data(:,15+((q-1)*18)); % S_displacement_y
    sensors_data(:,9,q)=data(:,16+((q-1)*18)); % S_displacement_z
    sensors_data(:,10,q)=data(:,17+((q-1)*18)); % Mx
    sensors_data(:,11,q)=data(:,18+((q-1)*18)); % My
    sensors_data(:,12,q)=data(:,19+((q-1)*18)); % Mz
    sensors_data(:,13,q)=data(:,6+((q-1)*18)); % TimeStamp
end
    init_time_sensors = sensors_data(1,13,:);
for q = 1:total_number
    sensors_time(:,q) = (sensors_data(:,13,q) - init_time_sensors(q))*1e-9;
end

% calculo do desfasamento de time_stamp entre servos e sensores
for q = 1:total_number
    delay_time(q) = (sensors_data(1,13,q)-humanoid_state(1,14))*1e-9;
end
% anulamento de desfasamento
for q = 1:total_number
    sensors_time(:,q) = sensors_time(:,q)+delay_time(q);
end

% % % % % % Grafs!!!! % % % % % % %


% rotacao dos joelhos - eixo GLOBAL Y - angluar_displacement
figure
plot(humanoid_time,humanoid_state(:,5),'b',sensors_time(:,5),-rad2deg(sensors_data(:,9,5)),'r',sensors_time(:,6),rad2deg(sensors_data(:,9,6)),'k')
title('Rotacao do joelho direito - eixo Y _ ang_displacement')

figure
plot(humanoid_time,humanoid_state(:,6),'b',sensors_time(:,7),rad2deg(sensors_data(:,9,7)),'r',sensors_time(:,8),-rad2deg(sensors_data(:,9,8)),'k')
title('Rotacao do joelho esquerdo - eixo Y _ ang_displacement')

figure
plot(humanoid_time,humanoid_state(:,5),'b',sensors_time(:,5), sensors_data(:,6,5),'r',sensors_time(:,6), sensors_data(:,6,6),'g')
title('Rotacao do joelho direito - eixo Y - velocidade angular')
% rotacao do pe - eixo GLOBAL Y
figure
plot(humanoid_time,humanoid_state(:,4),'b',sensors_time(:,8),rad2deg(sensors_data(:,9,8)),'r',sensors_time(:,8),moving_average(rad2deg(sensors_data(:,9,8)),5),'g')
title('Rotacao do pe - eixo Y _ ang_displacement')
% comportamento da anca...


figure
title('quatro sensores da anca - velocidade angular')
hold on
plot(sensors_time(:,1),sensors_data(:,6,1),'b')
plot(sensors_time(:,2),sensors_data(:,5,2),'r')
plot(sensors_time(:,3),-sensors_data(:,5,3),'k')
plot(sensors_time(:,4),-sensors_data(:,6,4),'g')

% VELOCIDADE ANGULAR em torno do eixo GLOBAL Y

figure
title('quatro sensores da anca - angular displacement')
hold on
plot(sensors_time(:,1),rad2deg(sensors_data(:,9,1)),'b')
plot(sensors_time(:,2),rad2deg(sensors_data(:,8,2)),'r')
plot(sensors_time(:,3),rad2deg(-sensors_data(:,8,3)),'k')
plot(sensors_time(:,4),rad2deg(-sensors_data(:,9,4)),'g')

figure
title('quatro sensores da anca - aceleracao linear, Z global')
hold on
plot(sensors_time(:,1),sensors_data(:,2,1),'b')
plot(sensors_time(:,2),-sensors_data(:,3,2),'r')
plot(sensors_time(:,3),-sensors_data(:,3,3),'k')
plot(sensors_time(:,4),sensors_data(:,2,4),'g')

%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

%% LOOKING FOR 'INSPIRATION'

clc ,close all , clear all

zz_angular_displacement = 'movezz_angular_displacement.csv';
humanoid_state_zz = 'movezz_humanoid_state.csv';

sensors_data = [];
humanoid_state = [];
init_time_sensors = [];
init_time_humanoid = [];
humanoid_time = [];
sensors_time = [];
delay_time = [];


% obtencao dos dados dos servomotores -
% 11,21,12,22,13,23,15,25,16,26,31,32,tronco,TimeStamp
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

% obtencao dos dados dos sensors
% Ax,Ay,Az,Gx,Gy,Gz,displacementX,displacementY,displacementZ,Mx,My,Mz,TimeStamp
importfile(zz_angular_displacement);
total_number = data(1,21);
for q = 1:total_number
    sensors_data(:,1,q)=data(:,8+((q-1)*18)); % S_Ax
    sensors_data(:,2,q)=data(:,9+((q-1)*18)); % S_Ay
    sensors_data(:,3,q)=data(:,10+((q-1)*18)); % S_Az
    sensors_data(:,4,q)=data(:,11+((q-1)*18)); % S_Gx
    sensors_data(:,5,q)=data(:,12+((q-1)*18)); % S_Gy
    sensors_data(:,6,q)=data(:,13+((q-1)*18)); % S_Gz
    sensors_data(:,7,q)=data(:,14+((q-1)*18)); % S_displacement_x
    sensors_data(:,8,q)=data(:,15+((q-1)*18)); % S_displacement_y
    sensors_data(:,9,q)=data(:,16+((q-1)*18)); % S_displacement_z
    sensors_data(:,10,q)=data(:,17+((q-1)*18)); % Mx
    sensors_data(:,11,q)=data(:,18+((q-1)*18)); % My
    sensors_data(:,12,q)=data(:,19+((q-1)*18)); % Mz
    sensors_data(:,13,q)=data(:,6+((q-1)*18)); % TimeStamp
end
    init_time_sensors = sensors_data(1,13,:);
for q = 1:total_number
    sensors_time(:,q) = (sensors_data(:,13,q) - init_time_sensors(q))*1e-9;
end

% Values from accelerometers to get the offset for each one of them
accel_max_min_val(:,:,1) = [-266 252
                            -256 263
                            -255 264];

accel_max_min_val(:,:,2) = [-257 260
                            -255 265
                            -264 256];
                        
accel_max_min_val(:,:,3) = [-274 254
                            -253 265
                            -260 260];
                        
accel_max_min_val(:,:,4) = [-258 260
                            -258 268
                            -259 248];

accel_max_min_val(:,:,5) = [-264 253
                            -265 273
                            -253 250];

accel_max_min_val(:,:,6) = [-274 255
                            -258 265
                            -272 260];
                        
accel_max_min_val(:,:,7) = [-265 255
                            -255 265
                            -261 253];
                        
accel_max_min_val(:,:,8) = [-264 252
                            -248 255
                            -258 259];
                        
accel_max_min_val(:,:,9) = [-268 265
                            -255 271
                            -285 230];

% sensors's offsets determination
accel_offset = zeros(9,3);
for sensor_n = 1:size(accel_offset,1)
    accel_offset(sensor_n,1) = (accel_max_min_val(1,1,sensor_n)+accel_max_min_val(1,2,sensor_n))/2;
    accel_offset(sensor_n,2) = (accel_max_min_val(2,1,sensor_n)+accel_max_min_val(2,2,sensor_n))/2;
    accel_offset(sensor_n,3) = (accel_max_min_val(3,1,sensor_n)+accel_max_min_val(3,2,sensor_n))/2;
end


% figure
% title('quatro sensores da anca - aceleracao linear, Z global')
% hold on
% plot(sensors_time(:,1),sensors_data(:,2,1)+accel_offset(1,2),'b')
% plot(sensors_time(:,2),-sensors_data(:,3,2)+accel_offset(2,3),'r')
% plot(sensors_time(:,3),-sensors_data(:,3,3)+accel_offset(3,3),'k')
% plot(sensors_time(:,4),sensors_data(:,2,4)+accel_offset(4,2),'g')

% figure
% hold on
% plot(humanoid_time,humanoid_state(:,6),'b',sensors_time(:,7),rad2deg(sensors_data(:,9,7)),'r',sensors_time(:,8),-rad2deg(sensors_data(:,9,8)),'k')
% plot(sensors_time(:,7),sensors_data(:,9,7),sensors_time(:,8),sensors_data(:,9,8))
% title('Rotacao do joelho esquerdo - eixo Y _ ang_displacement')
convert_fact = pi/180*0.069565;
time_interval = [0 ; diff(sensors_time(:,7))];
time_interval2 = [0 ; diff(sensors_time(:,8))];
displacement = cumsum(sensors_data(:,6,7).*convert_fact).*time_interval;
displacement = rad2deg(displacement);
displacement2 = cumsum(sensors_data(:,6,8).*convert_fact).*time_interval2;
displacement2 = rad2deg(displacement2);
displacement3 = displacement-displacement2;

% displacement from gyroscopes placed near the left knee
figure
subplot(2,2,1)
plot(humanoid_time,humanoid_state(:,6))
title('left knees servo data')
subplot(2,2,2)
plot(displacement3)
title('global displacement')
subplot(2,2,3)
plot(displacement)
title('displacement from sensor 7')
subplot(2,2,4)
plot(displacement2)
title('displacement from sensor 8')

% DCM Algorithm IMPLEMENTATION!!!

% dcm = [1 0 0
%        0 1 0
%        0 0 1];
% dcm = repmat(dcm,[1 1 total_number]);
% new_dcm = dcm ;
% roll = [] ;
% pitch = [] ;
% yaw = [] ;
% for q = 1:total_number
%     
%     for n = 2:size(sensors_data,1)
%         rot_increm_x(q) = sensors_data(n,4,q)*(sensors_time(n,q)-sensors_time(n-1,q));
%         rot_increm_y(q) = sensors_data(n,5,q)*(sensors_time(n,q)-sensors_time(n-1,q));
%         rot_increm_z(q) = sensors_data(n,6,q)*(sensors_time(n,q)-sensors_time(n-1,q));
%         
%         dcm(:,:,q) = [1 -rot_increm_z(q) rot_increm_y(q) ; rot_increm_z(q) 1 -rot_increm_x(q) ; -rot_increm_y(q) rot_increm_x(q) 1];
%         % error calculation
%         error = dot(dcm(1,:,q),dcm(2,:,q));
%         % error compensation
%         dcm(1,:,q) = dcm(1,:,q) - ((error/2)*dcm(2,:,q));
%         dcm(2,:,q) = dcm(2,:,q) - ((error/2)*dcm(1,:,q));
%         
%         % getting the tird line of dcm ortogonal with the lines obtained
%         % previously
%         dcm(3,:,q) = cross(dcm(1,:,q),dcm(2,:,q));
%         
%         %normalization
% %         dcm(1,:,q) = dcm(1,:,q) ./ sqrt(dcm(1,1,q)^2+dcm(1,2,q)^2+dcm(1,3,q)^2);
% %         dcm(2,:,q) = dcm(2,:,q) ./ sqrt(dcm(2,1,q)^2+dcm(2,2,q)^2+dcm(2,3,q)^2);
% %         dcm(3,:,q) = dcm(3,:,q) ./ sqrt(dcm(3,1,q)^2+dcm(3,2,q)^2+dcm(3,3,q)^2);
%         
% %         dcm(1,:,q) = (1/2) * (3 - dot(dcm(1,:,q),dcm(1,:,q)))*dcm(1,:,q);
% %         dcm(2,:,q) = (1/2) * (3 - dot(dcm(2,:,q),dcm(2,:,q)))*dcm(2,:,q);
% %         dcm(3,:,q) = (1/2) * (3 - dot(dcm(3,:,q),dcm(3,:,q)))*dcm(3,:,q);
% %         dcm(:,:,q)
%         % uniformization is missing HERE HERE HERE HERE HERE """
%         new_dcm(:,:,q) = new_dcm(:,:,q) * dcm(:,:,q);
%         
%         new_dcm(1,:,q) = new_dcm(1,:,q) ./ sqrt(new_dcm(1,1,q)^2+new_dcm(1,2,q)^2+new_dcm(1,3,q)^2);
%         new_dcm(2,:,q) = new_dcm(2,:,q) ./ sqrt(new_dcm(2,1,q)^2+new_dcm(2,2,q)^2+new_dcm(2,3,q)^2);
%         new_dcm(3,:,q) = new_dcm(3,:,q) ./ sqrt(new_dcm(3,1,q)^2+new_dcm(3,2,q)^2+new_dcm(3,3,q)^2);
% %         dcm(:,:,q)
%         new_dcm(:,:,q)
%         
%         roll(n,q) = atan2(new_dcm(3,2,q),new_dcm(3,3,q))*360/2/pi;
%         pitch(n,q) = asin(new_dcm(3,1,q))*360/2/pi;
%         yaw(n,q) = atan2(new_dcm(2,1,q),new_dcm(1,1,q))*360/2/pi;
%         
% 	
%     end
% end
% 
% figure
% hold on
% plot(roll(:,5),'r')
% plot(pitch(:,5),'g')
% plot(yaw(:,5),'k')
























%% Getting the force cells data... and other stuff
clear all , close all, clc
% the 4th column is the empty one!!
force_cells_1 = 'force_cells_1_ZZ.csv';
force_cells_2 = 'force_cells_2_ZZ.csv';
humanoid_state_zz = 'movezz_humanoid_state.csv';
zz_angular_displacement = 'movezz_angular_displacement.csv';
convert_fact = pi/180*0.069565;

importfile(force_cells_1);
time_1 = (data(:,1)-data(1,1))*1e-9;
force_1(:,1) = data(:,5);
force_1(:,2) = data(:,6);
force_1(:,3) = data(:,7);
force_1(:,4) = data(:,8);

init_time = data(1,1);

importfile(force_cells_2);
time_2 = (data(:,1)-data(1,1))*1e-9;
force_2(:,1) = data(:,5);
force_2(:,2) = data(:,6);
force_2(:,3) = data(:,7);
force_2(:,4) = data(:,8);

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) - init_time)*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

% obtencao dos dados dos sensors
% Ax,Ay,Az,Gx,Gy,Gz,displacementX,displacementY,displacementZ,Mx,My,Mz,TimeStamp
importfile(zz_angular_displacement);
total_number = data(1,21);
for q = 1:total_number
    sensors_data(:,1,q)=data(:,8+((q-1)*18)); % S_Ax
    sensors_data(:,2,q)=data(:,9+((q-1)*18)); % S_Ay
    sensors_data(:,3,q)=data(:,10+((q-1)*18)); % S_Az
    sensors_data(:,4,q)=data(:,11+((q-1)*18)); % S_Gx
    sensors_data(:,5,q)=data(:,12+((q-1)*18)); % S_Gy
    sensors_data(:,6,q)=data(:,13+((q-1)*18)); % S_Gz
    sensors_data(:,7,q)=data(:,14+((q-1)*18)); % S_displacement_x
    sensors_data(:,8,q)=data(:,15+((q-1)*18)); % S_displacement_y
    sensors_data(:,9,q)=data(:,16+((q-1)*18)); % S_displacement_z
    sensors_data(:,10,q)=data(:,17+((q-1)*18)); % Mx
    sensors_data(:,11,q)=data(:,18+((q-1)*18)); % My
    sensors_data(:,12,q)=data(:,19+((q-1)*18)); % Mz
    sensors_data(:,13,q)=data(:,6+((q-1)*18)); % TimeStamp
end
    init_time_sensors = sensors_data(1,13,:);
for q = 1:total_number
    sensors_time(:,q) = (sensors_data(:,13,q) - init_time)*1e-9;
end

% Values from accelerometers to get the offset for each one of them
accel_max_min_val(:,:,1) = [-266 252
                            -256 263
                            -255 264];

accel_max_min_val(:,:,2) = [-257 260
                            -255 265
                            -264 256];
                        
accel_max_min_val(:,:,3) = [-274 254
                            -253 265
                            -260 260];
                        
accel_max_min_val(:,:,4) = [-258 260
                            -258 268
                            -259 248];

accel_max_min_val(:,:,5) = [-264 253
                            -265 273
                            -253 250];

accel_max_min_val(:,:,6) = [-274 255
                            -258 265
                            -272 260];
                        
accel_max_min_val(:,:,7) = [-265 255
                            -255 265
                            -261 253];
                        
accel_max_min_val(:,:,8) = [-264 252
                            -248 255
                            -258 259];
                        
accel_max_min_val(:,:,9) = [-268 265
                            -255 271
                            -285 230];

% sensors's offsets determination
accel_offset = zeros(9,3);
for sensor_n = 1:size(accel_offset,1)
    accel_offset(sensor_n,1) = (accel_max_min_val(1,1,sensor_n)+accel_max_min_val(1,2,sensor_n))/2;
    accel_offset(sensor_n,2) = (accel_max_min_val(2,1,sensor_n)+accel_max_min_val(2,2,sensor_n))/2;
    accel_offset(sensor_n,3) = (accel_max_min_val(3,1,sensor_n)+accel_max_min_val(3,2,sensor_n))/2;
end


% subplot(1,2,1)
hold on
% plot(time_1,force_1(:,1),'b')
% plot(time_1,force_1(:,2),'r')
% plot(time_1,force_1(:,3),'g')
% plot(time_1,force_1(:,4),'k')
% subplot(1,2,2)
% hold on



plot(humanoid_time,humanoid_state(:,3),'r','LineWidth',3)

plot(humanoid_time(1:(end-1)),diff(humanoid_state(:,3)),'k','LineWidth',3) %servo 12

plot(sensors_time(:,6),sensors_data(:,6,6)*convert_fact*1.2,'y','LineWidth',4) % Gz_S6
plot(sensors_time(:,5),sensors_data(:,6,5)*convert_fact*1.2,'b','LineWidth',4) % Gz_S5

% plot(time_1,moving_average(force_1(:,1),200),'b')
% plot(time_1,moving_average(force_1(:,2),200),'r')
% plot(time_1,moving_average(force_1(:,3),200),'g')
% plot(time_1,moving_average(force_1(:,4),200),'k')
% legend('servo 12 - angle','servo 12 - speed','gyro 6 - Z','cell 1','cell 2','cell 3','cell4')
legend('servo 12 - angle','servo 12 - speed','gyro 6 - Z','gyro 5 - Z')

figure
hold on
plot(humanoid_time,humanoid_state(:,4),'r','LineWidth',3)

plot(humanoid_time(1:(end-1)),diff(humanoid_state(:,4)),'k','LineWidth',3) %servo 22

plot(sensors_time(:,8),sensors_data(:,6,8)*convert_fact*1.2,'y','LineWidth',4) % Gz_S8
plot(sensors_time(:,7),sensors_data(:,6,7)*convert_fact*1.2,'b','LineWidth',4) % Gz_S7
legend('servo 22 - angle','servo 22 - speed','gyro 8 - Z','gyro 7 - Z')

% plot(sensors_time(:,6),sensors_data(:,5,6)*convert_fact,'c','LineWidth',4)
% plot(sensors_time(:,6),sensors_data(:,4,6)*convert_fact,'r','LineWidth',4)
% subplot(1,3,3)
% hold on
% plot(time_2,force_2(:,1),'b')
% plot(time_2,force_2(:,2),'r')
% plot(time_2,force_2(:,3),'g')
% plot(time_2,force_2(:,4),'k')



%%


% % % % % % % %% - UP & DOWN
% % % % % % % clc , clear all
% % % % % % % 
% % % % % % % sensors_up_down = 'movexx_sensors.csv';
% % % % % % % humanoid_state_up_down = 'movexx_humanoid_state.csv';
% % % % % % % importfile(sensors_up_down);
% % % % % % % 
% % % % % % % total_number = data(1,18);
% % % % % % % sensors_data = [];
% % % % % % % humanoid_state = [];
% % % % % % % init_time_sensors = [];
% % % % % % % init_time_humanoid = [];
% % % % % % % for q = 1:total_number
% % % % % % %     for n=1:1:size(data,1)
% % % % % % %         sensors_data(n,1,q)=data(n,8+((q-1)*14)); % S_Ax
% % % % % % %         sensors_data(n,2,q)=data(n,9+((q-1)*14)); % S_Ay
% % % % % % %         sensors_data(n,3,q)=data(n,10+((q-1)*14)); % S_Az
% % % % % % %         sensors_data(n,4,q)=data(n,11+((q-1)*14)); % S_Mx
% % % % % % %         sensors_data(n,5,q)=data(n,12+((q-1)*14)); % S_My
% % % % % % %         sensors_data(n,6,q)=data(n,13+((q-1)*14)); % S_Mz
% % % % % % %         sensors_data(n,7,q)=data(n,14+((q-1)*14)); % S_Gx
% % % % % % %         sensors_data(n,8,q)=data(n,15+((q-1)*14)); % S_Gy
% % % % % % %         sensors_data(n,9,q)=data(n,16+((q-1)*14)); % S_Gz
% % % % % % %         sensors_data(n,10,q)=data(n,6+((q-1)*14))*1e-9; % TimeStamp
% % % % % % %         if n==1
% % % % % % %             init_time_sensors(q) = sensors_data(n,10,q);
% % % % % % %         end
% % % % % % %         sensors_data(n,10,q) = sensors_data(n,10,q) - init_time_sensors(q);
% % % % % % %     end
% % % % % % % end
% % % % % % % 
% % % % % % % importfile(humanoid_state_up_down);
% % % % % % % 
% % % % % % % for r = 1:size(data,1)
% % % % % % %     humanoid_state(r,1) = data(r,18); % servo 11
% % % % % % %     humanoid_state(r,2) = data(r,19); % servo 21
% % % % % % %     humanoid_state(r,3) = data(r,20); % servo 12
% % % % % % %     humanoid_state(r,4) = data(r,21); % servo 22
% % % % % % %     humanoid_state(r,5) = data(r,22); % servo 13
% % % % % % %     humanoid_state(r,6) = data(r,23); % servo 23
% % % % % % %     humanoid_state(r,7) = data(r,24); % servo 15
% % % % % % %     humanoid_state(r,8) = data(r,25); % servo 25
% % % % % % %     humanoid_state(r,9) = data(r,26); % servo 16
% % % % % % %     humanoid_state(r,10) = data(r,27); % servo 26
% % % % % % %     humanoid_state(r,11) = data(r,28); % servo 31
% % % % % % %     humanoid_state(r,12) = data(r,29); % servo 32
% % % % % % %     humanoid_state(r,13) = data(r,30); % servo (rotacao_tronco)
% % % % % % %     humanoid_state(r,14) = data(r,3); % TimeStamp
% % % % % % %     if r == 1
% % % % % % %         init_time_humanoid = humanoid_state(r,14);
% % % % % % %     end
% % % % % % %     humanoid_state(r,14) = (humanoid_state(r,14) - init_time_humanoid)*1e-9;
% % % % % % % end
% % % % % % % 
% % % % % % % 
% % % % % % % plot(sensors_data(:,10,1),-sensors_data(:,9,1),'b')
% % % % % % % hold on
% % % % % % % plot(sensors_data(:,10,1),-sensors_data(:,8,2),'r')
% % % % % % % plot(sensors_data(:,10,1),sensors_data(:,8,3),'k')
% % % % % % % plot(sensors_data(:,10,1),sensors_data(:,9,4),'g')
% % % % % % % figure
% % % % % % % plot(sensors_data(:,10,1),sensors_data(:,9,5))
% % % % % % % figure
% % % % % % % plot(sensors_data(:,10,1),sensors_data(:,9,6))
% % % % % % % figure
% % % % % % % plot(sensors_data(:,10,1),sensors_data(:,9,7))
% % % % % % % figure
% % % % % % % plot(sensors_data(:,10,1),sensors_data(:,9,8))
% % % % % % % figure
% % % % % % % plot(sensors_data(:,10,1),sensors_data(:,9,9))
% % % % % % % %% - SAGITAL
% % % % % % % clc , clear all
% % % % % % % 
% % % % % % % sensors_sagital = 'moveyy_sensors.csv';
% % % % % % % humanoid_state_sagital = 'moveyy_humanoid_state.csv';
% % % % % % % importfile(sensors_sagital);
% % % % % % % 
% % % % % % % total_number = data(1,18);
% % % % % % % sensors_data = [];
% % % % % % % humanoid_state = [];
% % % % % % % init_time_sensors = [];
% % % % % % % init_time_humanoid = [];
% % % % % % % for q = 1:total_number
% % % % % % %     for n=1:1:size(data,1)
% % % % % % %         sensors_data(n,1,q)=data(n,8+((q-1)*14)); % S_Ax
% % % % % % %         sensors_data(n,2,q)=data(n,9+((q-1)*14)); % S_Ay
% % % % % % %         sensors_data(n,3,q)=data(n,10+((q-1)*14)); % S_Az
% % % % % % %         sensors_data(n,4,q)=data(n,11+((q-1)*14)); % S_Mx
% % % % % % %         sensors_data(n,5,q)=data(n,12+((q-1)*14)); % S_My
% % % % % % %         sensors_data(n,6,q)=data(n,13+((q-1)*14)); % S_Mz
% % % % % % %         sensors_data(n,7,q)=data(n,14+((q-1)*14)); % S_Gx
% % % % % % %         sensors_data(n,8,q)=data(n,15+((q-1)*14)); % S_Gy
% % % % % % %         sensors_data(n,9,q)=data(n,16+((q-1)*14)); % S_Gz
% % % % % % %         sensors_data(n,10,q)=data(n,6+((q-1)*14))*1e-9; % TimeStamp
% % % % % % %         if n==1
% % % % % % %             init_time_sensors(q) = sensors_data(n,10,q);
% % % % % % %         end
% % % % % % %         sensors_data(n,10,q) = sensors_data(n,10,q) - init_time_sensors(q);
% % % % % % %     end
% % % % % % % end
% % % % % % % 
% % % % % % % importfile(humanoid_state_sagital);
% % % % % % % 
% % % % % % % for r = 1:size(data,1)
% % % % % % %     humanoid_state(r,1) = data(r,18); % servo 11
% % % % % % %     humanoid_state(r,2) = data(r,19); % servo 21
% % % % % % %     humanoid_state(r,3) = data(r,20); % servo 12
% % % % % % %     humanoid_state(r,4) = data(r,21); % servo 22
% % % % % % %     humanoid_state(r,5) = data(r,22); % servo 13
% % % % % % %     humanoid_state(r,6) = data(r,23); % servo 23
% % % % % % %     humanoid_state(r,7) = data(r,24); % servo 15
% % % % % % %     humanoid_state(r,8) = data(r,25); % servo 25
% % % % % % %     humanoid_state(r,9) = data(r,26); % servo 16
% % % % % % %     humanoid_state(r,10) = data(r,27); % servo 26
% % % % % % %     humanoid_state(r,11) = data(r,28); % servo 31
% % % % % % %     humanoid_state(r,12) = data(r,29); % servo 32
% % % % % % %     humanoid_state(r,13) = data(r,30); % servo (rotacao_tronco)
% % % % % % %     humanoid_state(r,14) = data(r,3)*1e-9; % TimeStamp
% % % % % % %     if r == 1
% % % % % % %         init_time_humanoid = humanoid_state(r,14) * 1e9-;
% % % % % % %     end
% % % % % % %     humanoid_state(r,14) = humanoid_state(r,14) - init_time_humanoid;
% % % % % % % end
