%% PLOTS....

%% RAZORS !!!!!
%% parado! - UTILIZADO NA TESE ESCRITA
clear all, close all, clc
save_id = 0;

location = 'Best';
% titulo ='Braço Robótico Parado';
CSVfilename = 'still_0_0_0_RAZOR.csv';
directory = '~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/';
gyro_x = 'still_0_0_0_gyro_X_RAZOR.jpg';
gyro_y = 'still_0_0_0_gyro_Y_RAZOR.jpg';
gyro_z = 'still_0_0_0_gyro_Z_RAZOR.jpg';
acel_x = 'still_0_0_0_acel_X_RAZOR.jpg';
acel_y = 'still_0_0_0_acel_Y_RAZOR.jpg';
acel_z = 'still_0_0_0_acel_Z_RAZOR.jpg';
gyroX_name=strcat(directory,gyro_x);
gyroY_name=strcat(directory,gyro_y);
gyroZ_name=strcat(directory,gyro_z);
acelX_name=strcat(directory,acel_x);
acelY_name=strcat(directory,acel_y);
acelZ_name=strcat(directory,acel_z);
% titulo = 'Movimentos Rotação';
font=17;
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... e nao a 40

a=figure
plot(time,S_Gy/16.384,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-3 3],'YTick',(-3:0.5:3))
% set(gca,'ylim',[0 1])
grid on
title('Braço Robótico Parado - eixo X - sensor A')
legend('Giroscópio','Location',location)
xlabel('Tempo (segundos)')
ylabel('Graus por segundo')
if save_id == 1
    saveas(a,gyroX_name);
end
std_gyro_1 = std(S_Gy/16.384)
mean_1 = mean(S_Gy/16.384)
b=figure
plot(time,S_Gx/16.384,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-3 3],'YTick',(-3:0.5:3))
grid on
title('Braço Robótico Parado - eixo Y - sensor A')
legend('Giroscópio','Location',location)
xlabel('Tempo (segundos)')
ylabel('Graus por segundo')
if save_id == 1
    saveas(b,gyroY_name);
end
std_gyro_2 = std(S_Gx/16.384)
mean_2 = mean(S_Gx/16.384)

c=figure
plot(time,S_Gz/16.384,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-3 3],'YTick',(-3:0.5:3))
grid on
title('Braço Robótico Parado - eixo Z - sensor A')
legend('Giroscópio','Location',location)
xlabel('Tempo (segundos)')
ylabel('Graus por segundo')
if save_id == 1
    saveas(c,gyroZ_name);
end
std_gyro_3 = std(S_Gz/16.384)
mean_3 = mean(S_Gz/16.384)

d=figure
plot(time,S_Ay/256,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-2 2],'YTick',(-2:0.4:2))
grid on
title('Braço Robótico Parado - eixo X - sensor A')
legend('Acelerómetro','Location',location)
xlabel('Tempo (segundos)')
ylabel('Aceleração (g)')
if save_id == 1
    saveas(d,acelX_name);
end
std_gyro_4 = std(S_Ay/256)
mean_4 = mean(S_Ay/256)

e=figure
plot(time,S_Ax/256,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-2 2],'YTick',(-2:0.4:2))
grid on
title('Braço Robótico Parado - eixo Y - sensor A')
legend('Acelerómetro','Location',location)
xlabel('Tempo (segundos)')
ylabel('Aceleração (g)')
if save_id == 1
    saveas(e,acelY_name);
end
std_gyro_5 = std(S_Ax/256)
mean_5 = mean(S_Ax/256)

f=figure
plot(time,S_Az/256,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-2 2],'YTick',(-2:0.4:2))
grid on
title('Braço Robótico Parado - eixo Z - sensor A')
legend('Acelerómetro','Location',location)
xlabel('Tempo (segundos)')
ylabel('Aceleração (g)')
if save_id == 1
    saveas(f,acelZ_name);
end
std_gyro_6 = std(S_Az/256)
mean_6 = mean(S_Az/256)


figure
hold on, grid on
magn = plot(time,sqrt(S_Mx.^2+S_My.^2+S_Mz.^2)*0.92*1e-3,'b','LineWidth',2)
title('Parado - Magnetómetro - Sensor A','FontSize',17)
ylabel('Gauss','FontSize',17)
xlabel('Tempo [seg]','FontSize',17)
set(gca,'FontSize',17)
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)); %pk dados extraídos a 20Hz... e nao a 40

hold on
plot(time,sqrt(S_Mx.^2+S_My.^2+S_Mz.^2)*0.91*1e-3,'r','LineWidth',2)
legend('motores desligados','motores ligados','FontSize',17,'Location','SouthEast')
if save_id == 1
    saveas(magn,'~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/magn_mag_par_A.jpg')
end

%% MOVIMENTOS TRANSLAÇÃO... 1000 mm/s - UTILIZADO NA TESE ESCRITA

clear all, close all, clc
save_id = 0;

location = 'NorthEast';

directory = '~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/';
gyro_x = 'trans_0_0_0_gyro_X_RAZOR.jpg';
gyro_y = 'trans_0_0_0_gyro_Y_RAZOR.jpg';
gyro_z = 'trans_0_0_0_gyro_Z_RAZOR.jpg';
acel_x = 'trans_0_0_0_acel_X_RAZOR.jpg';
acel_y = 'trans_0_0_0_acel_Y_RAZOR.jpg';
acel_z = 'trans_0_0_0_acel_Z_RAZOR.jpg';
gyroX_name=strcat(directory,gyro_x);
gyroY_name=strcat(directory,gyro_y);
gyroZ_name=strcat(directory,gyro_z);
acelX_name=strcat(directory,acel_x);
acelY_name=strcat(directory,acel_y);
acelZ_name=strcat(directory,acel_z);
font=17;
CSVfilename = 'trans_0_0_0_RAZOR.csv';
% titulo = 'Movimentos Rotação';
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... e nao a 40



% Acelerómetros - POSICAO
a=figure
[AX,y1,y2]=plotyy(time,F_X,time,S_Ay/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - X - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font)
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
set(AX(1),'ylim',[0 1000],'YTick',(0:200:1000))
legend('Posição Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(a,acelX_name);
end
std_1 = std(S_Ay/256)

b=figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Ax/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Y - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font)
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
legend('Posição Braço Robótico','Acelerómetro','Location','NorthEast')
if save_id == 1
    saveas(b,acelY_name);
end
std_2 = std(S_Ax/256)


c=figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Az/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Z - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font)
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
legend('Posição Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(c,acelZ_name);
end
std_3 = std(S_Az/256)

% Giroscópios - POSICAO
d=figure
[AX,y1,y2]=plotyy(time,F_X,time,S_Gy/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - X - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font)
set(AX(2),'ylim',[-3 3],'YTick',(-3:0.5:3))
set(AX(1),'ylim',[0 1000],'YTick',(0:200:1000))
legend('Posição Braço Robótico','Giroscópio','Location','South','FontSize',font)
if save_id == 1
    saveas(d,gyroX_name);
end
std_4 = std(S_Gy/16.384)

e=figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Gx/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Y - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-3 3],'YTick',(-3:0.5:3))
legend('Posição Braço Robótico','Giroscópio','Location','NorthEast')
if save_id == 1
    saveas(e,gyroY_name);
end
std_5 = std(S_Gx/16.384)


f=figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Gz/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Z - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-3 3],'YTick',(-3:0.5:3))
legend('Posição Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(f,gyroZ_name);
end
std_6 = std(S_Gz/16.384)

figure
hold on, grid on
magn = plot(time,sqrt(S_Mx.^2+S_My.^2+S_Mz.^2)*(0.92*1e-3),'b','LineWidth',2)
title('Movimento de Translação - Magnetómetro - Sensor A','FontSize',17)
legend('Magnetómetro - magnitude','FontSize',17)
ylabel('Gauss','FontSize',17)
xlabel('Tempo [seg]','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(magn,'~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/magn_mag_trans_A.jpg')
end


Roll = zeros(1,length(time));
Pitch = zeros(1,length(time));
xmag = S_My./sqrt(S_Mx.^2+S_My.^2+S_Mz.^2);
ymag = S_Mx./sqrt(S_Mx.^2+S_My.^2+S_Mz.^2);
zmag = S_Mz./sqrt(S_Mx.^2+S_My.^2+S_Mz.^2);
yaw=[];
for a = 1:length(time)
    yaw = [ yaw atan2( -(ymag(a)*cos(Roll(a)) + zmag(a)*sin(Roll(a))) , (xmag(a)*cos(Pitch(a)) + ymag(a)*sin(Pitch(a))*sin(Roll(a))+zmag(a)*sin(Pitch(a))*cos(Roll(a))))]; 
end
yaw = rad2deg(yaw);
figure
hold on, grid on
title('Movimento de Translação - Magnetómetro - Sensor A','FontSize',17)
yaw_magn = plot(time,yaw,'Color',[0 0 0.8],'LineWidth',2)
% plot(time,yaw,'ko','LIneWidth',2)
xlabel('Tempo [seg]','FontSize',17)
ylabel('Variação Angular [\circ]','FontSize',17)
legend('Yaw - Magentómetro','FontSize',17)
set(gca,'FontSize',17)
% axis([0 10 -200 200])
if save_id == 1
    saveas(yaw_magn,'~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/magn_yaw_trans_A.jpg')
end

graph = figure
hold on, grid on
time_d = diff(time');
fanuc_speed = diff(F_X/1000)./time_d;
for w = 1:length(time_d)
time2(w) = time(w)+(time_d(w)/2);
end
time_dd = diff(time2');
fanuc_accel = diff(fanuc_speed)./time_dd;
for q = 1:length(time_dd)
time3(q) = time2(q)+(time_dd(q)/2);
end
plot(time3,fanuc_accel/9.81,'b','LineWidth',2)
plot(time,S_Ay/256,'g','LineWidth',2)

%% MOVIMENTOS TRANSLAÇÃO... 100 mm/s

clear all, close all, clc
save_id = 0;

location = 'NorthEast';

directory = '~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/';
gyro_x = 'trans_0_0_0_gyro_X_RAZOR_100.jpg';
gyro_y = 'trans_0_0_0_gyro_Y_RAZOR_100.jpg';
gyro_z = 'trans_0_0_0_gyro_Z_RAZOR_100.jpg';
acel_x = 'trans_0_0_0_acel_X_RAZOR_100.jpg';
acel_y = 'trans_0_0_0_acel_Y_RAZOR_100.jpg';
acel_z = 'trans_0_0_0_acel_Z_RAZOR_100.jpg';
gyroX_name=strcat(directory,gyro_x);
gyroY_name=strcat(directory,gyro_y);
gyroZ_name=strcat(directory,gyro_z);
acelX_name=strcat(directory,acel_x);
acelY_name=strcat(directory,acel_y);
acelZ_name=strcat(directory,acel_z);

font=17;
CSVfilename = 'trans_0_0_0_RAZOR2.csv';
% titulo = 'Movimentos Rotação';
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... e nao a 40



% Acelerómetros - POSICAO
a=figure
[AX,y1,y2]=plotyy(time,F_X,time,S_Ay/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - X - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
legend('Posição Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(a,acelX_name);
end
std_1 = std(S_Ay/256)

b=figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Ax/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Y - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
legend('Posição Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(b,acelY_name);
end
std_2 = std(S_Ax/256)


c=figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Az/256)
grid on
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
title('Movimento Translação - Z - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
legend('Posição Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(c,acelZ_name);
end
std_3 = std(S_Az/256)

% Giroscópios - POSICAO
d=figure
[AX,y1,y2]=plotyy(time,F_X,time,S_Gy/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - X - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
legend('Posição Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(d,gyroX_name);
end
std_4 = std(S_Gy/16.384)

e=figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Gx/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Y - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
legend('Posição Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(e,gyroY_name);
end
std_5 = std(S_Gx/16.384)


f=figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Gz/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Z - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
legend('Posição Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(f,gyroZ_name);
end
std_6 = std(S_Gz/16.384)

%% MOVIMENTOS ROTACAO... 200 º/s - UTILIZADO NA TESE ESCRITA

clear all, close all, clc
save_id = 0;

location = 'NorthEast';

directory = '~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/';
gyro_x = 'rot_0_0_0_gyro_X_RAZOR.jpg';
gyro_y = 'rot_0_0_0_gyro_Y_RAZOR.jpg';
gyro_z = 'rot_0_0_0_gyro_Z_RAZOR.jpg';
acel_x = 'rot_0_0_0_acel_X_RAZOR.jpg';
acel_y = 'rot_0_0_0_acel_Y_RAZOR.jpg';
acel_z = 'rot_0_0_0_acel_Z_RAZOR.jpg';
gyroX_name=strcat(directory,gyro_x);
gyroY_name=strcat(directory,gyro_y);
gyroZ_name=strcat(directory,gyro_z);
acelX_name=strcat(directory,acel_x);
acelY_name=strcat(directory,acel_y);
acelZ_name=strcat(directory,acel_z);

font=17;
CSVfilename = 'rotacao_RAZOR.csv';
% titulo = 'Movimentos Rotação';
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... e nao a 40

% Acelerómetro - ORIENTACAO
a=figure
[AX,y1,y2]=plotyy(time,F_W,time,S_Ax/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - X - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro - eixo Y','Location','Best')
if save_id == 1
    saveas(a,acelX_name);
end
std_1 = std(S_Ax/256)

b=figure
[AX,y1,y2]=plotyy(time,F_P,time,S_Ay/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Y - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro - eixo X','Location','NorthEast')
if save_id == 1
    saveas(b,acelY_name);
end
std_2 = std(S_Ay/256)

c=figure
[AX,y1,y2]=plotyy(time,F_R,time,S_Az/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Z - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font)
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.5:2))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro - eixo Z','Location','NorthEast')
if save_id == 1
    saveas(c,acelZ_name);
end
std_3 = std(S_Az/256)

% Giroscópios - ORIENTACAO
d=figure
[AX,y1,y2]=plotyy(time,F_W,time,-S_Gy/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - X - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font)
set(AX(2),'ylim',[-180 180],'YTick',(-180:40:180))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio - eixo X','Location','SouthWest')
if save_id == 1
    saveas(d,gyroX_name);
end

e=figure
[AX,y1,y2]=plotyy(time,F_P,time,-S_Gx/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Y - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font)
set(AX(2),'ylim',[-180 180],'YTick',(-180:40:180))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio - eixo Y','Location','NorthEast')
if save_id == 1
    saveas(e,gyroY_name);
end

f=figure
[AX,y1,y2]=plotyy(time,F_R,time,-S_Gz/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Z - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font)
set(AX(2),'ylim',[-180 180],'YTick',(-180:40:180))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio - eixo Z','Location','NorthEast')
if save_id == 1
    saveas(f,gyroZ_name);
end

F_W_speed = diff(F_W)./diff(time');
time_d = diff(time');
for w = 1:length(diff(time)')
time2(w) = time(w)+(time_d(w)/2);
end
final=figure
hold on, grid on

plot(time2,F_W_speed,'b','LineWidth',2)
plot(time,-S_Gy/16.384,'g','LineWidth',2)

xlabel('Tempo (segundos)','FontSize',font)
ylabel('Graus por segundo [\circ/s]','FontSize',font)

legend('Braço Robótico','Giroscópio - eixo X','Location','SouthWest')
set(gca,'FontSize',font)
if save_id == 1
    saveas(final,'~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/rot_0_0_0_gyro_X_RAZOR_derivado.jpg');
end
%% MOVIMENTOS ROTACAO... 50 º/s

clear all, close all, clc
save_id = 0;

location = 'NorthEast';

directory = '~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/';
gyro_x = 'rot_0_0_0_gyro_X_RAZOR_50.jpg';
gyro_y = 'rot_0_0_0_gyro_Y_RAZOR_50.jpg';
gyro_z = 'rot_0_0_0_gyro_Z_RAZOR_50.jpg';
acel_x = 'rot_0_0_0_acel_X_RAZOR_50.jpg';
acel_y = 'rot_0_0_0_acel_Y_RAZOR_50.jpg';
acel_z = 'rot_0_0_0_acel_Z_RAZOR_50.jpg';
gyroX_name=strcat(directory,gyro_x);
gyroY_name=strcat(directory,gyro_y);
gyroZ_name=strcat(directory,gyro_z);
acelX_name=strcat(directory,acel_x);
acelY_name=strcat(directory,acel_y);
acelZ_name=strcat(directory,acel_z);

font=17;
CSVfilename = 'rotacao_RAZOR2.csv';
% titulo = 'Movimentos Rotação';
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... e nao a 40

% Acelerómetro - ORIENTACAO
a=figure
[AX,y1,y2]=plotyy(time,F_W,time,S_Ax/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - X - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-1.5 1.5],'YTick',(-1.5:0.5:1.5))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(a,acelX_name);
end
std_1 = std(S_Ax/256)

b=figure
[AX,y1,y2]=plotyy(time,F_P,time,S_Ay/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Y - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-1.5 1.5],'YTick',(-1.5:0.5:1.5))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(b,acelY_name);
end
std_2 = std(S_Ay/256)

c=figure
[AX,y1,y2]=plotyy(time,F_R,time,S_Az/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Z - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-1.5 1.5],'YTick',(-1.5:0.5:1.5))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(c,acelZ_name);
end
std_3 = std(S_Az/256)

% Giroscópios - ORIENTACAO
d=figure
[AX,y1,y2]=plotyy(time,F_W,time,S_Gy/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - X - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-60 60],'YTick',(-60:20:60))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(d,gyroX_name);
end
std_4 = std(S_Gy/16.384)

e=figure
[AX,y1,y2]=plotyy(time,F_P,time,S_Gx/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Y - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-60 60],'YTick',(-60:20:60))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Posição Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(e,gyroY_name);
end
std_5 = std(S_Gx/16.384)

f=figure
[AX,y1,y2]=plotyy(time,F_R,time,S_Gz/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Z - Sensor A')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-60 60],'YTick',(-60:20:60))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(f,gyroZ_name);
end
std_6 = std(S_Gz/16.384)

%% POLOLUs
%% parado!  - UTILIZADO NA TESE ESCRITA
clear all, close all, clc
save_id = 0;

location='Best';
directory = '~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/';
gyro_x = 'still_0_0_0_gyro_X_POLOLU.jpg';
gyro_y = 'still_0_0_0_gyro_Y_POLOLU.jpg';
gyro_z = 'still_0_0_0_gyro_Z_POLOLU.jpg';
acel_x = 'still_0_0_0_acel_X_POLOLU.jpg';
acel_y = 'still_0_0_0_acel_Y_POLOLU.jpg';
acel_z = 'still_0_0_0_acel_Z_POLOLU.jpg';
gyroX_name=strcat(directory,gyro_x);
gyroY_name=strcat(directory,gyro_y);
gyroZ_name=strcat(directory,gyro_z);
acelX_name=strcat(directory,acel_x);
acelY_name=strcat(directory,acel_y);
acelZ_name=strcat(directory,acel_z);
font=17;
CSVfilename = 'still_0_0_0_POLOLU.csv';
% titulo = 'Movimentos Rotação';
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... e nao a 40

a=figure
plot(time,S_Gx/16.384,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-3 3],'YTick',(-3:0.5:3))

grid on
title('Braço Robótico Parado eixo X - Sensor B')
legend('Giroscópio','Location',location)
xlabel('Tempo (segundos)')
ylabel('Graus por segundo')
if save_id == 1
    saveas(a,gyroX_name);
end
std_1 = std(S_Gx/16.384)
mean_1 = mean(S_Gx/16.384)

b=figure
plot(time,S_Gy/16.384,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-3 3],'YTick',(-3:0.5:3))
grid on
title('Braço Robótico Parado eixo Y - Sensor B')
legend('Giroscópio','Location',location)
xlabel('Tempo (segundos)')
ylabel('Graus por segundo')
if save_id == 1
    saveas(b,gyroY_name);
end
std_2 = std(S_Gy/16.384)
mean_2 = mean(S_Gy/16.384)

c=figure
plot(time,S_Gz/16.384,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-3 3],'YTick',(-3:0.5:3))
grid on
title('Braço Robótico Parado eixo Z - Sensor B')
legend('Giroscópio','Location',location)
xlabel('Tempo (segundos)')
ylabel('Graus por segundo')
if save_id == 1
    saveas(c,gyroZ_name);
end
std_3 = std(S_Gz/16.384)
mean_3 = mean(S_Gz/16.384)

d=figure
plot(time,S_Ax/256,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-2 2],'YTick',(-2:0.4:2))
grid on
title('Braço Robótico Parado eixo X - Sensor B')
legend('Acelerómetro','Location',location)
xlabel('Tempo (segundos)')
ylabel('Aceleração (g)')
if save_id == 1
    saveas(d,acelX_name);
end
std_4 = std(S_Ax/256)
mean_4 = mean(S_Ax/256)

e=figure
plot(time,S_Ay/256,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-2 2],'YTick',(-2:0.4:2))
grid on
title('Braço Robótico Parado eixo Y - Sensor B')
legend('Acelerómetro','Location',location)
xlabel('Tempo (segundos)')
ylabel('Aceleração (g)')
if save_id == 1
    saveas(e,acelY_name);
end
std_5 = std(S_Ay/256)
mean_5 = mean(S_Ay/256)

f=figure
plot(time,S_Az/256,'LineWidth',2)
set(gca,'FontSize',font,'ylim',[-2 2],'YTick',(-2:0.4:2))
grid on
title('Braço Robótico Parado eixo Z - Sensor B')
legend('Acelerómetro','Location',location)
xlabel('Tempo (segundos)')
ylabel('Aceleração (g)')
if save_id == 1
    saveas(f,acelZ_name);
end
std_6 = std(S_Az/256)
mean_6 = mean(S_Az/256)

figure
hold on, grid on
magn = plot(time,sqrt(S_Mx.^2+S_My.^2+S_Mz.^2)*(0.91*1e-3),'b','LineWidth',2)
title('Parado - Magnitudes do Magnetómetro - Sensor B','FontSize',17)
ylabel('Gauss','FontSize',17)
xlabel('Tempo [seg]','FontSize',17)
set(gca,'FontSize',17)
axis([0 10 4e-4 6e-4])
set(gca,'YTick',(4e-4:0.5e-4:6e-4))
CSVfilename = 'still_M_0_0_0_POLOLU.csv';
% titulo = 'Movimentos Rotação';
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... e nao a 40

hold on
plot(time,sqrt(S_Mx.^2+S_My.^2+S_Mz.^2)*(0.91*1e-3),'r','LineWidth',2)
legend('motores desligados','motores ligados','FontSize',17)

if save_id == 1
    saveas(magn,'~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/magn_mag_par_B.jpg')
end
%% MOVIMENTOS TRANSLAÇÃO... 1000 mm/s - UTILIZADO NA TESE ESCRITA

clear all, close all, clc
save_id = 0;

directory = '~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/';
gyro_x = 'trans_0_0_0_gyro_X_POLOLU.jpg';
gyro_y = 'trans_0_0_0_gyro_Y_POLOLU.jpg';
gyro_z = 'trans_0_0_0_gyro_Z_POLOLU.jpg';
acel_x = 'trans_0_0_0_acel_X_POLOLU.jpg';
acel_y = 'trans_0_0_0_acel_Y_POLOLU.jpg';
acel_z = 'trans_0_0_0_acel_Z_POLOLU.jpg';
gyroX_name=strcat(directory,gyro_x);
gyroY_name=strcat(directory,gyro_y);
gyroZ_name=strcat(directory,gyro_z);
acelX_name=strcat(directory,acel_x);
acelY_name=strcat(directory,acel_y);
acelZ_name=strcat(directory,acel_z);

font=17;
CSVfilename = 'trans_0_0_0_POLOLU2.csv';
% titulo = 'Movimentos Rotação';
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... e nao a 40



% Acelerómetros - POSICAO
a=figure;
[AX,y1,y2]=plotyy(time,F_X,time,S_Ax/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - X - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font)
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
set(AX(1),'ylim',[0 1000],'YTick',(0:100:1000))
legend('Posição Braço Robótico','Acelerómetro','Location','NorthEast')
if save_id == 1
    saveas(a,acelX_name);
end
std_1 = std(S_Ax/256)

b=figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Ay/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Y - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
set(AX(1),'ylim',[-200 600],'YTick',(-200:100:600))
legend('Posição Braço Robótico','Acelerómetro','Location','NorthEast')
if save_id == 1
    saveas(b,acelY_name);
end
std_2 = std(S_Ay/256)


c=figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Az/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Z - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
set(AX(1),'ylim',[0 1500],'YTick',(0:300:1500))
legend('Posição Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(c,acelZ_name);
end
std_3 = std(S_Az/256)

% Giroscópios - POSICAO
d=figure
[AX,y1,y2]=plotyy(time,F_X,time,S_Gy/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - X - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-3 3],'YTick',(-3:0.5:3))
set(AX(1),'ylim',[0 1000],'YTick',(0:100:1000))
legend('Posição Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(d,gyroX_name);
end
std_4 = std(S_Gy/16.384)

e=figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Gx/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Y - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-3 3],'YTick',(-3:0.5:3))
set(AX(1),'ylim',[-200 600],'YTick',(-200:100:600))
legend('Posição Braço Robótico','Giroscópio','Location','NorthEast')
if save_id == 1
    saveas(e,gyroY_name);
end
std_5 = std(S_Gx/16.384)


f=figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Gz/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Z - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-3 3],'YTick',(-3:0.5:3))
set(AX(1),'ylim',[500 1500],'YTick',(500:300:1500))
legend('Posição Braço Robótico','Giroscópio','Location','NorthEast')
if save_id == 1
    saveas(f,gyroZ_name);
end
std_6 = std(S_Gz/16.384)

figure
hold on, grid on
magn = plot(time,sqrt(S_Mx.^2+S_My.^2+S_Mz.^2)*(0.91*1e-3),'b','LineWidth',2)
title('Movimento de Translação - Magnetómetro - Sensor B','FontSize',17)
legend('Magnetómetro - magnitude','FontSize',17)
ylabel('Gauss','FontSize',17)
xlabel('Tempo [seg]','FontSize',17)
set(gca,'FontSize',17)
% axis([0 10 -50 50])
% set(gca,'YTick',(-50:20:50))
if save_id == 1
    saveas(magn,'~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/magn_mag_trans_B.jpg')
end

Roll = zeros(1,length(time));
Pitch = zeros(1,length(time));
xmag = S_Mx./sqrt(S_Mx.^2+S_My.^2+S_Mz.^2);
ymag = S_My./sqrt(S_Mx.^2+S_My.^2+S_Mz.^2);
zmag = S_Mz./sqrt(S_Mx.^2+S_My.^2+S_Mz.^2);
yaw=[];
for a = 1:length(time)
    yaw = [ yaw atan2( -(ymag(a)*cos(Roll(a)) + zmag(a)*sin(Roll(a))) , (xmag(a)*cos(Pitch(a)) + ymag(a)*sin(Pitch(a))*sin(Roll(a))+ zmag(a)*sin(Pitch(a))*cos(Roll(a))) )]; 
end
yaw = rad2deg(yaw);
figure
hold on, grid on
title('Movimento de Translação - Magnetómetro - Sensor B','FontSize',17)
yaw_magn = plot(time,yaw,'Color',[0 0 0.8],'LineWidth',2)
% plot(time,yaw,'ko','LIneWidth',2)
xlabel('Tempo [seg]','FontSize',17)
ylabel('Variação Angular [\circ]','FontSize',17)
legend('Yaw - Magentómetro','FontSize',17)
set(gca,'FontSize',17)
if save_id == 1
    saveas(yaw_magn,'~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/magn_yaw_trans_B.jpg')
end

%% MOVIMENTOS TRANSLAÇÃO... 100 mm/s

clear all, close all, clc
save_id = 0;

directory = '~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/';
gyro_x = 'trans_0_0_0_gyro_X_POLOLU_100.jpg';
gyro_y = 'trans_0_0_0_gyro_Y_POLOLU_100.jpg';
gyro_z = 'trans_0_0_0_gyro_Z_POLOLU_100.jpg';
acel_x = 'trans_0_0_0_acel_X_POLOLU_100.jpg';
acel_y = 'trans_0_0_0_acel_Y_POLOLU_100.jpg';
acel_z = 'trans_0_0_0_acel_Z_POLOLU_100.jpg';
gyroX_name=strcat(directory,gyro_x);
gyroY_name=strcat(directory,gyro_y);
gyroZ_name=strcat(directory,gyro_z);
acelX_name=strcat(directory,acel_x);
acelY_name=strcat(directory,acel_y);
acelZ_name=strcat(directory,acel_z);

font=17;
CSVfilename = 'trans_0_0_0_POLOLU.csv';
% titulo = 'Movimentos Rotação';
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... e nao a 40



% Acelerómetros - POSICAO
a=figure;
[AX,y1,y2]=plotyy(time,F_X,time,S_Ax/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - X - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
legend('Posição Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(a,acelX_name);
end
std_1 = std(S_Ax/256)

b=figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Ay/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Y - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
legend('Posição Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(b,acelY_name);
end
std_2 = std(S_Ay/256)


c=figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Az/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Z - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
legend('Posição Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(c,acelZ_name);
end
std_3 = std(S_Az/256)

% Giroscópios - POSICAO
d=figure
[AX,y1,y2]=plotyy(time,F_X,time,S_Gy/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - X - Sensor B')
xlabel('Tempo (segundos)')
set(get(AX(1),'Ylabel'),'String','Posição(mm)') 
set(get(AX(2),'Ylabel'),'String','Graus por segundo') 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
legend('Posição Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(d,gyroX_name);
end
std_4 = std(S_Gy/16.384)

e=figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Gx/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Y - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
legend('Posição Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(e,gyroY_name);
end
std_5 = std(S_Gx/16.384)


f=figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Gz/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Translação - Z - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Posição(mm)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
legend('Posição Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(f,gyroZ_name);
end
std_6 = std(S_Gz/16.384)

%% MOVIMENTOS ROTACAO... 200 º/s - UTILIZADO NA TESE ESCRITA

clear all, close all, clc
save_id = 0;

directory = '~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/';
gyro_x = 'rot_0_0_0_gyro_X_POLOLU.jpg';
gyro_y = 'rot_0_0_0_gyro_Y_POLOLU.jpg';
gyro_z = 'rot_0_0_0_gyro_Z_POLOLU.jpg';
acel_x = 'rot_0_0_0_acel_X_POLOLU.jpg';
acel_y = 'rot_0_0_0_acel_Y_POLOLU.jpg';
acel_z = 'rot_0_0_0_acel_Z_POLOLU.jpg';
gyroX_name=strcat(directory,gyro_x);
gyroY_name=strcat(directory,gyro_y);
gyroZ_name=strcat(directory,gyro_z);
acelX_name=strcat(directory,acel_x);
acelY_name=strcat(directory,acel_y);
acelZ_name=strcat(directory,acel_z);

font=17;
CSVfilename = 'rotacao_POLOLU2.csv';
% titulo = 'Movimentos Rotação';
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... e nao a 40

% Acelerómetro - ORIENTACAO
a=figure
[AX,y1,y2]=plotyy(time,F_W,time,S_Ay/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - X - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro - eixo Y','Location','NorthWest')
if save_id == 1
    saveas(a,acelX_name);
end
std_1 = std(S_Ay/256)

b=figure
[AX,y1,y2]=plotyy(time,F_P,time,S_Ax/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Y - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro - eixo X','Location','NorthWest')
if save_id == 1
    saveas(b,acelY_name);
end
std_2 = std(S_Ax/256)

c=figure
[AX,y1,y2]=plotyy(time,F_R,time,S_Az/256-1)
grid on
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
title('Movimento Rotação - Z - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font)
set(AX(2),'ylim',[-2 2],'YTick',(-2:0.4:2))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro','Location','SouthEast')
if save_id == 1
    saveas(c,acelZ_name);
end
std_3 = std(S_Az/256)

% Giroscópios - ORIENTACAO
d=figure
[AX,y1,y2]=plotyy(time,F_W,time,S_Gx/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - X - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font)
set(AX(2),'ylim',[-180 180],'YTick',(-180:40:180))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio - eixo X','Location','SouthWest')
if save_id == 1
    saveas(d,gyroX_name);
end
std_4 = std(S_Gx/16.384)

e=figure
[AX,y1,y2]=plotyy(time,F_P,time,S_Gy/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Y - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font)
set(AX(2),'ylim',[-180 180],'YTick',(-180:40:180))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio - eixo Y','Location','NorthEast')
if save_id == 1
    saveas(e,gyroY_name);
end
std_5 = std(S_Gy/16.384)

f=figure

[AX,y1,y2]=plotyy(time,F_R,time,S_Gz/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Z - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font)
set(AX(2),'ylim',[-180 180],'YTick',(-180:40:180))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio - eixo Z','Location','Best')
if save_id == 1
    saveas(f,gyroZ_name);
end
std_6 = std(S_Gz/16.384)
%% MOVIMENTOS ROTACAO... 50 º/s

clear all, close all, clc
save_id = 0; 
directory = '~/Dropbox/Dissertacao/tese_escrita2/Imagens/sensors_images/';
gyro_x = 'rot_0_0_0_gyro_X_POLOLU_50.jpg';
gyro_y = 'rot_0_0_0_gyro_Y_POLOLU_50.jpg';
gyro_z = 'rot_0_0_0_gyro_Z_POLOLU_50.jpg';
acel_x = 'rot_0_0_0_acel_X_POLOLU_50.jpg';
acel_y = 'rot_0_0_0_acel_Y_POLOLU_50.jpg';
acel_z = 'rot_0_0_0_acel_Z_POLOLU_50.jpg';
gyroX_name=strcat(directory,gyro_x);
gyroY_name=strcat(directory,gyro_y);
gyroZ_name=strcat(directory,gyro_z);
acelX_name=strcat(directory,acel_x);
acelY_name=strcat(directory,acel_y);
acelZ_name=strcat(directory,acel_z);

font=17;
CSVfilename = 'rotacao_POLOLU.csv';
% titulo = 'Movimentos Rotação';
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... e nao a 40

% Acelerómetro - ORIENTACAO
a=figure
[AX,y1,y2]=plotyy(time,F_W,time,S_Ay/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - X - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font) 
set(AX(2),'ylim',[-1.5 1.5],'YTick',(-1.5:0.3:1.5))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro - eixo Y','Location','Best')
if save_id == 1
    saveas(a,acelX_name);
end
std_1 = std(S_Ay/256)

b=figure
[AX,y1,y2]=plotyy(time,F_P,time,S_Ax/256)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Y - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font)
set(AX(2),'ylim',[-1 2],'YTick',(-1:0.3:2))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro - eixo X','Location','Best')
if save_id == 1
    saveas(b,acelY_name);
end
std_2 = std(S_Ax/256)

c=figure
[AX,y1,y2]=plotyy(time,F_R,time,S_Az/256-1)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Z - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Orientação (graus)','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Aceleração (g)','FontSize',font)
set(AX(2),'ylim',[-0.5 1.5],'YTick',(-0.5:0.3:1.5))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Acelerómetro','Location','Best')
if save_id == 1
    saveas(c,acelZ_name);
end
std_3 = std(S_Az/256)

% Giroscópios - ORIENTACAO
d=figure
[AX,y1,y2]=plotyy(time,F_W,time,S_Gx/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - X - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-50 50],'YTick',(-50:10:50))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio','Location','NorthWest')
if save_id == 1
    saveas(d,gyroX_name);
end
std_4 = std(S_Gx/16.384)

e=figure
[AX,y1,y2]=plotyy(time,F_P,time,S_Gy/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Y - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-50 50],'YTick',(-50:10:50))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(e,gyroY_name);
end
std_5 = std(S_Gy/16.384)

f=figure
[AX,y1,y2]=plotyy(time,F_R,time,S_Gz/16.384)
set(AX,'FontSize',font)
set(y1,'LineWidth',2)
set(y2,'LineWidth',2)
grid on
title('Movimento Rotação - Z - Sensor B')
xlabel('Tempo (segundos)','FontSize',font)
set(get(AX(1),'Ylabel'),'String','Graus','FontSize',font) 
set(get(AX(2),'Ylabel'),'String','Graus por segundo','FontSize',font) 
set(AX(2),'ylim',[-50 50],'YTick',(-50:10:50))
set(AX(1),'ylim',[-20 100],'YTick',(-20:20:100))
legend('Orientação Braço Robótico','Giroscópio','Location','Best')
if save_id == 1
    saveas(f,gyroZ_name);
end
std_6 = std(S_Gz/16.384)
