


%% plotting the curves...

clear all, close all, clc

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

 %% show rotation movements with gyro's reactions
 close all
 
 subplot(2,3,1)

 plot(time,F_W,'k')
 legend('Fanuc end-effect rotation (degrees)')
 xlabel('Time (s)')
 ylabel('(mm)')
 subplot(2,3,4)
 plot(time,S_Gy,'r')
 legend('Gyro reaction')
 xlabel('Time (s)')
 ylabel('(º/s)')
 
%  figure
 subplot(2,3,2)
 plot(time,F_P,'k')
 legend('Fanuc end-effect rotation (degrees)')
 xlabel('Time (s)')
 ylabel('(mm)')
 subplot(2,3,5)
 plot(time,S_Gx,'r')
 legend('Gyro reaction')
 xlabel('Time (s)')
 ylabel('(º/s)')
%  figure
 subplot(2,3,3)
 plot(time,F_R,'k')
 legend('Fanuc end-effect rotation (degrees)')
 xlabel('Time (s)')
 ylabel('(mm)')
 subplot(2,3,6)
 plot(time,S_Gz,'r')
 legend('Gyro reaction')
 xlabel('Time (s)')
 ylabel('(º/s)')
 %% show rotation movements with acel's reactions
 close all
 
 subplot(2,3,1)
 plot(time,F_W,'k')
 legend('Fanuc end-effect rotation (degrees)')
 subplot(2,3,4)
 plot(time,S_Ax,'r')
 legend('Accel reaction')
 xlabel('Time (seconds)')
 
%  figure
 subplot(2,3,2)
 plot(time,F_P,'k')
 legend('Fanuc end-effect rotation (degrees)')
 subplot(2,3,5)
 plot(time,S_Ay,'r')
 legend('Accel reaction')
 xlabel('Time (seconds)')
 
%  figure
 subplot(2,3,3)
 plot(time,F_R,'k')
 legend('Fanuc end-effect rotation (degrees)')
 subplot(2,3,6)
 plot(time,S_Az,'r')
 legend('Accel reaction')
 xlabel('Time (seconds)')
  
 %% show translaction movements with acel's reactions
%  close all
%  hold on
figure
 subplot(2,3,1)
 plot(time,F_X,'k')
 legend('Fanuc end-effect translaction (mm)')
 subplot(2,3,4)
 plot(time,S_Ay,'r')
 legend('Accel x reaction')
 xlabel('Time (seconds)')
 
%  figure
 subplot(2,3,2)
 plot(time,F_Y,'k')
 legend('Fanuc end-effect translaction (mm)')
 subplot(2,3,5)
 plot(time,S_Ax,'r')
 legend('Accel y reaction')
 xlabel('Time (seconds)')
 
%  figure
 subplot(2,3,3)
 plot(time,F_Z,'k')
 legend('Fanuc end-effect translaction (mm)')
 subplot(2,3,6)
 plot(time,S_Az,'r')
 legend('Accel z reaction')
 xlabel('Time (seconds)')
  
 %% show translaction movements with gyro's reactions
 close all
 
 subplot(2,3,1)
 plot(time,F_X,'k')
 legend('Fanuc end-effect translaction (mm)')
 subplot(2,3,4)
 plot(time,S_Gx,'r')
 legend('Gyro x reaction')
 xlabel('Time (seconds)')
 
%  figure
 subplot(2,3,2)
 plot(time,F_Y,'k')
 legend('Fanuc end-effect translaction (mm)')
 subplot(2,3,5)
 plot(time,S_Gy,'r')
 legend('Gyro y reaction')
 xlabel('Time (seconds)')
 
%  figure
 subplot(2,3,3)
 plot(time,F_Z,'k')
 legend('Fanuc end-effect translaction (mm)')
 subplot(2,3,6)
 plot(time,S_Gz,'r')
 legend('Gyro z reaction')
 xlabel('Time (seconds)')
 
 %% integraçao dos valores do giroscópio para dar a variação ângular
 
 % POLOLU
 
 close all 
 % Pode - se observar o drift do giroscópio perfeitamente....
 %TENHO QUE AFINAR O PARÂMETRO DE ESCALA DO GIROSCÓPIO
 %pois assim assumo sempre que o valor q ele mostra é sempre 90º mum pico..
 %o que pode nao corresponder à realidade!!!!!!
 
 dps_digit = 14.286; % 14.286 é o equivalente a (1º/sec) conversão
 % utilizando a conversao... 2^15/2000 = 16.384
 subplot(1,3,1)
 rot=cumtrapz(S_Gx)*(1/20)/dps_digit; 
 plot(time,rot,'r',time,F_W,'b')
 xlabel('Time (s)')
 ylabel('degrees')
 subplot(1,3,2)
 rot=cumsum(S_Gy)*(1/20)/dps_digit;
 plot(time,rot,'r',time,F_P,'b')
 xlabel('Time (s)')
 ylabel('degrees')
 subplot(1,3,3)
 rot=cumsum(S_Gz)*(1/20)/dps_digit;
 plot(time,rot,'r',time,F_R,'b')
 xlabel('Time (s)')
 ylabel('degrees')
 legend('gyro calculated angles','fanuc angles')
 
 
 %% integraçao dos valores do giroscópio para dar a variação ângular
 
 % RAZOR
 
 close all 
 % Pode - se observar o drift do giroscópio perfeitamente....
 %TENHO QUE AFINAR O PARÂMETRO DE ESCALA DO GIROSCÓPIO
 %pois assim assumo sempre que o valor q ele mostra é sempre 90º mum pico..
 %o que pode nao corresponder à realidade!!!!!!
 
 subplot(1,3,1)
 rot=-cumsum(S_Gy)*(1/20)/14.375; % 14.375 é o equivalente a (1º/sec) conversão
 plot(time,rot,'r',time,F_W,'b')
 
 subplot(1,3,2)
 rot=-cumsum(S_Gx)*(1/20)/14.375;
 plot(time,rot,'r',time,F_P,'b')
 
 subplot(1,3,3)
 rot=-cumsum(S_Gz)*(1/20)/14.375;
 plot(time,rot,'r',time,F_R,'b')
 
 legend('gyro calculated angles','fanuc angles')
 
  %% integraçao dos valores do acelerómetro para dar a variação ângular
  
  % RAZOR
  
 subplot(2,3,1)
 rot=cumsum(cumsum(S_Ay/256)); % 14.375 é o equivalente a (1º/sec) conversão
 plot(time,rot,'r',time,F_X,'b')
 subplot(2,3,4)
 plot(time,S_Ay,'r',time,F_X,'b')
 
 subplot(2,3,2)
 rot=cumsum(cumsum(S_Ax/256));
 plot(time,rot,'r',time,F_Y,'b')
 subplot(2,3,5)
 plot(time,S_Ax,'r',time,F_Y,'b')
 
 subplot(2,3,3)
 rot=cumsum(cumsum(S_Az/256));
 plot(time,rot,'r',time,F_Z,'b')
 legend('acel calculated displacement','fanuc displacement')
  subplot(2,3,6)
 plot(time,S_Az,'r',time,F_Z,'b')
  legend('acel values','fanuc displacement')
 
  %% integraçao dos valores do acelerómetro para dar a variação ângular
  
  % POLOLU
  
 subplot(2,3,1)
 rot=cumsum(cumsum(S_Ay/256)); % 14.375 é o equivalente a (1º/sec) conversão
 plot(time,rot,'r',time,F_X,'b')
 subplot(2,3,4)
 plot(time,S_Ay,'r',time,F_X,'b')
 
 subplot(2,3,2)
 rot=cumsum(cumsum(S_Ax/256));
 plot(time,rot,'r',time,F_Y,'b')
 subplot(2,3,5)
 plot(time,S_Ax,'r',time,F_Y,'b')
 
 subplot(2,3,3)
 rot=cumsum(cumsum(S_Az/256));
 plot(time,rot,'r',time,F_Z,'b')
%  legend('acel calculated displacement','fanuc displacement')
  subplot(2,3,6)
 plot(time,S_Az,'r',time,F_Z,'b')
%   legend('acel values','fanuc displacement')
  
  
  %% mais um teste... 
  close all
  rot = cumtrapz((S_Az-mean(S_Az))/256*9.81);
  med = mean(S_Az);
  rot = rot - med;
  plot(time,rot,'b',time,F_Z,'r')
  
  
  
  %%
  
%   function disp_time_data = acc2disp(acc_time_data,dt)
% % acc_time_data should be acceleration amplitudes evenly spaced in time
% % dt in units of seconds per sample
%     N1 = length(acc_time_data);
%     N = 2^nextpow2(N1);
%     if N > N1
%         acc_time_data(N1+1:N) = 0; % pad array with 0's
%     end
%     df = 1 / (N*dt); % frequency increment
%     Nyq = 1 / (2*dt); % Nyquist frequency
%     acc_freq_data = fftshift(fft(acc_time_data));
%     disp_freq_data = zeros(size(acc_freq_data));
%     f = -Nyq:df:Nyq-df; % I think this is how fftshift organizes it
%     for i = 1 : N
%         if f(i) ~= 0
%             disp_freq_data(i) = acc_freq_data(i)/(2*pi*f(i)*sqrt(-1))^2;
%         else
%             disp_freq_data(i) = 0;
%         end
%     end
% %     return
%   end
%   disp_time_data = ifft(ifftshift(disp_freq_data));    
%   disp_time_data = disp_time_data(1:N1);
% %   return
%%
subplot(1,3,1)
plot(time,F_Y,'r',time,S_Ay,'b') 
subplot(1,3,2)
plot(time,fft(S_Ay*9.81/256-mean(S_Ay)),'r',time,S_Ay,'b',time,ifft(fft(S_Ay*9.81/256-mean(S_Ay))),'g')
subplot(1,3,3)
cena = cumsum(fftshift(fft(S_Ay/256*9.81)-mean(S_Ay/256*9.81)));
plot(time,cena,'r')
%% 
plot(time,nextpow2(S_Ax),time,S_Ax)

%% BAAAAAAAAAAAHHHHHHHhhhh - NADA DE JEITO!!!


close all

Gforce = S_Ax / 256;
SAMPLE_RATE = 1/20;
GRAVITYFT_SEC = 9.81;
velocity=[];
displacement=[];
for i=1:size(S_Ax,1)
    if i == 1
        velocity(i) = Gforce(i) * SAMPLE_RATE * GRAVITYFT_SEC; %// Initial velocity
        displacement(i) = velocity(i) * SAMPLE_RATE;% * INCH_FOOT;% // Initial Displacement
    else
        velocity(i) = velocity(i-1) + (Gforce(i) * SAMPLE_RATE * GRAVITYFT_SEC);% // Calculate running velocity into buffer
        displacement(i) = displacement(i-1) +(velocity(i) * SAMPLE_RATE);% * INCH_FOOT);% // Calculate running displacement into buffer
    end
end

plot(1:size(velocity,2),velocity,'r',1:size(displacement,2),displacement,'b')


%%
close all

x0 = 0;

dt = (1 / 20);
RC = 0.1;
alpha = dt / (RC + dt);
    
cena = S_Az - S_Az(1);
smoothed= zeros(length(S_Az),1);
for n = 1:size(S_Az,1)
 smoothed(n,1) = (alpha * cena(n,1)) + (1.0 - alpha) * x0;
 x0 = smoothed(n);
end

x02 = 0;
RC2 = 0.02
alpha2 = dt / (RC2 + dt);
smoothed2= zeros(length(S_Az),1);
for n = 1:size(S_Az,1)
 smoothed2(n,1) = (alpha2 * cena(n,1)) + (1.0 - alpha2) * x02;
 x02 = smoothed2(n);
end
cena = cumsum(cumsum(smoothed/256*9.81))/(20^2);
cena2 = cumsum(cumsum(smoothed2/256*9.81))/(20^2);

plot(time,S_Az,'b',time,smoothed,'r',time,smoothed2,'k')
figure
plot(time,cena,'g',time,F_Z,'k',time,cena2,'y')

figure
plot(time,S_Ax,'b',time,S_Ay,'r',time,S_Az,'g')
%% applying butter

[B A]= butter(2,0.05);

cena = S_Ax / 256 * 9.81;

fil = filter (B,A,cena);

cena2 = cumsum(cumsum(fil))/(400); % continua a dar barraca!!!!
% o q estarei a fazer mal ? ?????

plot(time,cena,'r',time,fil,'b',time,cena2,'k')

%% 
novosDados = S_Ay(1:60);
Ts=1/20; %tempo entre amostras
Fs = 1/Ts; % Frequ�ncia de amostragem
L=length(novosDados); %comprimento da amostra
NFFT = 2^nextpow2(L); % Next power of 2 from length of novosDados
Y = fft(novosDados,NFFT)/L;
f = Fs/2*linspace(0,1,NFFT/2+1);
% f0=f(2*abs(Y(1:NFFT/2+1))==max(2*abs(Y(1:NFFT/2+1))));
% w0=2*pi*f0;
out = 2*abs(Y(1:NFFT/2+1));
plot(f,out)
%% 
close all
subplot(2,1,1)
plot(time,filter(Num,1,S_Ax),'r')
subplot(2,1,2)
plot(time,S_Ax,'b')


%% 
plot(time,S_Ax,'b',time,fft(S_Ax),'r')


%% 
kFilteringFactor = 0.8;
cenas = S_Ax;
for n = 2:size(S_Ax,1)
    cenas(n)=S_Ax(n)*kFilteringFactor + S_Ax(n-1).*(1-kFilteringFactor);
end
% cenas = S_Ax.*kFilteringFactor + S_Ax.*(1-kFilteringFactor);

plot(time,S_Ax,'b',time,cenas,'r')


%% graficos de giroscópios... para adicionar na tese !!! - fanuc parado

plot(time,S_Gy)
title('Braço Robótico Parado - X')
legend('Giroscópio')
xlabel('tempo (segundos)')
ylabel('graus por segundo')

figure
plot(time,S_Gx)
title('Braço Robótico Parado - Y')
legend('Giroscópio')
xlabel('tempo (segundos)')
ylabel('graus por segundo')

figure
plot(time,S_Gz)
title('Braço Robótico Parado - Z')
legend('Giroscópio')
xlabel('tempo (segundos)')
ylabel('graus por segundo')

%% gráficos deacelerómetros para adicionar  - fanuc parado

plot(time,S_Ay/256)
title('Braço Robótico Parado - X')
legend('Acelerómetro - eixo X')
xlabel('tempo (segundos)')
ylabel('aceleração (g)')

figure
plot(time,S_Ax/256)
title('Braço Robótico Parado - Y')
legend('Acelerómetro - eixo Y')
xlabel('tempo (segundos)')
ylabel('aceleração (g)')

figure
plot(time,S_Az/256)
title('Braço Robótico Parado - Z')
legend('Acelerómetro - eixo Z')
xlabel('tempo (segundos)')
ylabel('aceleração (g)')


%% graficos de giroscópios... para adicionar na tese !!! - MOVIMENTOS TRANSLACAO

[AX,y1,y2]=plotyy(time,F_X,time,S_Gx)
title('Movimento Translação - X')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','posicao(mm)') 
set(get(AX(2),'Ylabel'),'String','graus por segundo') 
legend('Posição Braço Robótico','Giroscópio')


figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Gy)
title('Movimento Translação - Y')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','posicao(mm)') 
set(get(AX(2),'Ylabel'),'String','graus por segundo') 
legend('Posição Braço Robótico','Giroscópio')



figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Gz)
title('Movimento Translação - Z')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','posicao(mm)') 
set(get(AX(2),'Ylabel'),'String','graus por segundo') 
legend('Posição Braço Robótico','Giroscópio')


%% gráficos deacelerómetros para adicionar  - MOVIMENTOS TRANSLACAO


[AX,y1,y2]=plotyy(time,F_X,time,S_Ay/256)
title('Movimento Translação - X')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','posicao(mm)') 
set(get(AX(2),'Ylabel'),'String','aceleração (g)') 
legend('Posição Braço Robótico','Acelerómetro')


figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Ax/256)
title('Movimento Translação - Y')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','posicao(mm)') 
set(get(AX(2),'Ylabel'),'String','aceleração (g)') 
legend('Posição Braço Robótico','Acelerómetro')



figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Az/256)
title('Movimento Translação - Z')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','posicao(mm)') 
set(get(AX(2),'Ylabel'),'String','aceleração (g)') 
legend('Posição Braço Robótico','Acelerómetro')


%% graficos de giroscópios... para adicionar na tese !!! - MOVIMENTOS ROTACAO
[AX,y1,y2]=plotyy(time,F_W,time,S_Gy)
title('Movimento Translação - X')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','posicao(mm)') 
set(get(AX(2),'Ylabel'),'String','graus por segundo') 
legend('Posição Braço Robótico','Giroscópio')


figure
[AX,y1,y2]=plotyy(time,F_P,time,S_Gx)
title('Movimento Translação - Y')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','posicao(mm)') 
set(get(AX(2),'Ylabel'),'String','graus por segundo') 
legend('Posição Braço Robótico','Giroscópio')



figure
[AX,y1,y2]=plotyy(time,F_R,time,S_Gz)
title('Movimento Translação - Z')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','posicao(mm)') 
set(get(AX(2),'Ylabel'),'String','graus por segundo') 
legend('Posição Braço Robótico','Giroscópio')


%% gráficos de acelerómetros para adicionar  - MOVIMENTOS ROTACAO
[AX,y1,y2]=plotyy(time,F_X,time,S_Ay/256)
title('Movimento Rotação - X')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','orientação (graus)') 
set(get(AX(2),'Ylabel'),'String','aceleração (g)') 
legend('Posição Braço Robótico','Acelerómetro')


figure
[AX,y1,y2]=plotyy(time,F_Y,time,S_Ax/256)
title('Movimento Translação - Y')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','orientação (graus)') 
set(get(AX(2),'Ylabel'),'String','aceleração (g)') 
legend('Posição Braço Robótico','Acelerómetro')



figure
[AX,y1,y2]=plotyy(time,F_Z,time,S_Az/256)
title('Movimento Translação - Z')
xlabel('tempo (segundos')
set(get(AX(1),'Ylabel'),'String','orientação (graus)') 
set(get(AX(2),'Ylabel'),'String','graus por segundo') 
legend('Posição Braço Robótico','Acelerómetro')
 
%% 

[AX,y1,y2]=plotyy(time,F_X,time,S_Ax)
legend('cena','canona')
xlabel('time')
set(get(AX(1),'Ylabel'),'String','FANUCAS') 
set(get(AX(2),'Ylabel'),'String','SENSORZAO') 