clear all %#ok<CLSCR>
close all
clc

format long

filename = 'ensaio3a.csv';

dados = csvread(filename);

 hold on; grid on
 
 figure(1)
 
 tempo = ( dados(:,1) - dados(1,1) ) .* 1e-9 ; 
 
 velocity = dados(:,7);
 
 for i = 1 : size(velocity,1)
     if velocity(i) > 1023
         velocity(i) = ( velocity(i) - 1023 )  ; % sem dsitinguir o sentido
     end
 end
  
 position = dados(:,5);
 
 load = dados(:,8);
  for i = 1 : size(load,1)
     if load(i) > 1023
         load(i) = load(i) - 1023; % para não se distinguir o sentido
     end
  end
 
 plot(tempo,velocity * 0.111 * 6,'r.') % velocidade em degree per second

 plot(tempo,(position- 204 ) .* ( 180 / 615 ) ,'.'); % posiçao em graus

 plot(tempo,(100/1023).*load,'g.') %valor em percentagem
 
 xlabel('seconds')

 timestamp = zeros(size(dados,1)-1,1);
 posdif = zeros(size(dados,1)-1,1);
 realvel = zeros(size(dados,1)-1,1);
 

 for i = 1 : (size(dados,1)-1)
     
     timestamp(i) = ( dados(1+i,1) - dados(i,1) ) * 1e-9 ; % second
     
     posdif(i) = abs( (dados(1+i,5)) - dados(i,5) ) * (300/1024); % degree
     
     realvel(i) = ( posdif(i) / timestamp(i) ) / 6 ; % rpm
     
 end
 
 realvell = zeros( size(realvel,1) , 1);
 realvell2 = realvell;
 
 for ii = 6 : (size(realvel,1))
     
     A = [ realvel(ii-1), realvel(ii-2), realvel(ii-3), realvel(ii-4), realvel(ii-5) ] ;

     realvell(ii) = median( A , 2 ) ; % mediana dos ultimos 5 valores
     
     realvell2(ii) = sum(A) / 5 ; % media dos ultimos 5 valores
 end
 

 newtime = zeros(size(realvel,1),1);
 
 for n = 1 : (size(tempo) - 1)
     
     newtime(n) = (tempo(n) + tempo(n+1)) / 2 ; % como é a velocidade média, o instante de tempo é o médio entre dois instantes
 
 end
 
 plot(newtime, realvell * 6,'c')
 %plot(newtime, realvell2 * 6,'y')
 %plot(newtime, realvel * 6,'b')
 
 title('vel=200grau/s, peso=255g')
 legend('Velocidade [grau/s]','Posição [graus]','Carga [%]','Vel Mediana [grau/s]' )
 
 plot(tempo, 100 * ones(size(tempo,1),1) ,'r')
 plot(tempo, 180 * ones(size(tempo,1),1) ,'r')
 