%F_X = posicao x "end-effector" em mm
%F_Y = posicao y "end-effector" em mm
%F_Z = posicao z "end-effector" em mm
%F_W = orientacao "end-effector" no eixo x em graus
%F_P = orientacao "end-effector" no eixo y em graus
%F_R = orientacao "end-effector" no eixo z em graus

% S_R = sensor roll
% S_P = sensor pitch
% S_Y = sensor yaw
% S_Ax = acelerómetro x
% S_Ay = acelerómetro y
% S_Az = acelerómetro z
% S_Mx = magnetómetro x
% S_My = magnetómetro y
% S_Mz = magnetómetro z
% S_Gx = giroscópio x
% S_Gy = giroscópio y
% S_Gz = giroscópio z

%os valores dos giroscópios vem em [º/s]

%os valores dos acelerómetros divididos por 256 vem em g-force

% dados extraídos a 20Hz

%% pequenos pormenores a ter em conta...

% os valores dos eixos x e y dos giroscópios do sensor RAZOR,
% correspondem aos eixos do Fanuc y e x (há aqui uma troca)

% ainda neste sensor, a troca verifica-se também nos acelerómetros
% relativamente aos eixos x e y...

%estas duas alterações devem-se ao facto de o sistema de coordenadas
%do sensor RAZOR ter uma rotação em torno do eixo Z do fanuc de 90º
% ou seja... o eixo y do fanuc corresponde ao eixo x do sensor
% e o eixo x do fanuc corresponde ao eixo y do sensor


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


%%%%%%%%%%%%%%%%%%%%%%%%%
%%ROTACAO ....
%%%%%%%%%%%%%%%%%%%%%%%%%

% rotacoes realizadas em torno de eixo de cada vez pela ordem z,y,x

clear all, close all, clc
% VELOCIDADE DE ROTACAO = 200 º/s
CSVfilename = 'rotacao_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1)); %pk dados extraídos a 20Hz...

%%
clear all, close all, clc
% VELOCIDADE DE ROTACAO = 50 º/s
CSVfilename = 'rotacao_RAZOR2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));
plot(time,F_W,time,S_Gx)
%%
clear all, close all, clc
% VELOCIDADE DE ROTACAO = 50 º/s
CSVfilename = 'rotacao_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));
plot(time,F_W,time,S_Ay)
%%
clear all, close all, clc
% VELOCIDADE DE ROTACAO = 200 º/s
CSVfilename = 'rotacao_POLOLU2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%     TRANSLACAO  %%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% movimentos de translacao realizados apenas num eixo de cada vez

% os 3 números a seguir à paralavra trans indicam a orientação do
% "end-effector" do fanuc

%%
clear all, close all, clc
% VELOCIDADE DE TRANSLACAO = 1000 mm/s
CSVfilename = 'trans_0_0_0_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));
%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 100 mm/s
CSVfilename = 'trans_0_0_0_RAZOR2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 1000 mm/s
CSVfilename = 'trans_90_0_0_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 100 mm/s
CSVfilename = 'trans_90_0_0_RAZOR2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 1000 mm/s
CSVfilename = 'trans_90_90_0_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 100 mm/s
CSVfilename = 'trans_90_90_0_RAZOR2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 100 mm/s
CSVfilename = 'trans_0_0_0_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 1000 mm/s
CSVfilename = 'trans_0_0_0_POLOLU2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 100 mm/s
CSVfilename = 'trans_90_0_0_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 1000 mm/s
CSVfilename = 'trans_90_0_0_POLOLU2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 100 mm/s
CSVfilename = 'trans_90_90_0_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 1000 mm/s
CSVfilename = 'trans_90_90_0_POLOLU2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%% STILL TESTES %%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% os 3 números que se suguem à palavra still representam a orientação
% W,P,R do "end-effector" do Fanuc...

% quando o nome do ficheiro contém um "M" , os motores estão ligados
% aquando a realização dos testes, caso contrário estariam desligados

%%
clear all, close all, clc

CSVfilename = 'still_0_0_0_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

CSVfilename = 'still_M_0_0_0_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));


%%
clear all, close all, clc

CSVfilename = 'still_90_0_0_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));
%%
clear all, close all, clc

CSVfilename = 'still_M_90_0_0_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

CSVfilename = 'still_90_90_0_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

CSVfilename = 'still_M_90_90_0_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));
%%
clear all, close all, clc

CSVfilename = 'still_0_0_0_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

CSVfilename = 'still_M_0_0_0_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

CSVfilename = 'still_90_0_0_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

CSVfilename = 'still_M_90_0_0_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

CSVfilename = 'still_90_90_0_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

CSVfilename = 'still_M_90_90_0_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%% MOVIMENTOS 3D %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% nos movimentos de rotacao são movimentados os 3 eixos em simultâneo

% nos movimentos de translacao são movimentados os 3 eixos em simultâneo

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 100 mm/s

CSVfilename = 'trans3D_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE TRANSLACAO = 1000 mm/s

CSVfilename = 'trans3D_RAZOR2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

%VELOCIDADE DE TRANSLACAO = 100 mm/s

CSVfilename = 'trans3D_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

%VELOCIDADE DE TRANSLACAO = 1000 mm/s

CSVfilename = 'trans3D_POLOLU2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE ROTACAO = 50 º/s
CSVfilename = 'rotacao3D_RAZOR.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE ROTACAO = 200 º/s
CSVfilename = 'rotacao3D_RAZOR2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE ROTACAO = 50 º/s

CSVfilename = 'rotacao3D_POLOLU.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

%%
clear all, close all, clc

% VELOCIDADE DE ROTACAO = 200 º/s

CSVfilename = 'rotacao3D_POLOLU2.csv';
importfile(CSVfilename);
F_X = data(:,5);
F_Y = data(:,6);
F_Z = data(:,7);
F_W = data(:,8);
F_P = data(:,9);
F_R = data(:,10);

S_R = data(:,11);
S_P = data(:,12);
S_Y = data(:,13);
S_Ax = data(:,14);
S_Ay = data(:,15);
S_Az = data(:,16);
S_Mx = data(:,17);
S_My = data(:,18);
S_Mz = data(:,19);
S_Gx = data(:,20);
S_Gy = data(:,21);
S_Gz = data(:,22);

time = (1/20)* (1:size(data,1));

