function [fi,Ar,Bo]=defasagem(A,f,ce,cs)
% USO:          [fi,Ar,Bo]=defasagem(A,f,ce,cs)
%
% Nome:         defasagem
%
% Versão:       v1.0
%
% Autores:      Geraldo F. Silveira e João B. R. do Val
%               silveira@dsce.fee.unicamp.br
%
% Data:         26/05/2001
%
% Argumentos:   1o. matriz A: corresponde aa matriz dos resultados obtidos
%                 na experiencia. Formato: Sample, Time, ... 
%                 ... [Commanded Pos, Encoder 1 Pos, Encoder 2 Pos, ...
%                 ...  Encoder 2 Pos, Control Effort] 
%                 Ordem e no. de colunas entre [] podem variar.
% 
%               2o. f: frequencia da senoide do sinal de controle.
%               3o. ce: coluna da matriz A que contem o sinal de entrada.
%               4o. cs: coluna da matriz A que contem o sinal de saída.  
%
% Finalidade:   calculo da defasagem (fi) entre duas ondas senoidais,
%               da relação de ganhos entre os sinais de saída e de entrada (Ar)
%               e da amplitude media do sinal de saída (Bo).
%
%

format short g, format compact
close all;
figure(1);
plota(A,ce,cs);
figure(2)
plot(A(:,1),A(:,cs));
ylabel('encoder position (counts)');
xlabel('amostras');
hold on; grid on;


%pega kT e o (k+n)T automaticamente
disp('Marque o início do regime diretamente no gráfico...');
[i1,y1]=ginput(1);
i1=floor(i1);

%%% calcula e retira o valor DC do sinal de entrada
t_ini1 = A(i1,2); int_tot_raw = A(end,2) - t_ini1;
t_fim1 = A(end,2) - rem(int_tot_raw,(1/f));
% vetor contendo sinal de entrada em um número inteiro de períodos:
ind= find((t_ini1<=A(:,2))&(A(:,2)<t_fim1)); 
u_raw = A(ind,ce); 
% calcula o período das amostras, o valor DC e retira a polarização:
Ts = (t_fim1-t_ini1)/size(ind,1);
disp('Período de amostragem ...'), disp(Ts)
dc_u= (sum(u_raw)- (u_raw(1)+u_raw(end))/2)/size(ind,1); % método do trapézio
disp('Valor DC do sinal de entrada ...'), disp(dc_u)
A(:,ce)=A(:,ce) - dc_u;

% busca refinada do cruzamento de zero
u1=A(i1,ce); i_ini=i1; 
while sign(u1)==sign(A(i_ini+1,ce))
   i_ini=i_ini+1;
   u1=A(i_ini,ce);
end         % ao sair (i_ini,u1) é o ultimo valor antes do cruzamento
i_fim=A(end,1); u2=A(i_fim,ce); 
while sign(u2)==sign(A(i_fim-1,ce))
   i_fim=i_fim-1;
   u2=A(i_fim,ce);
end
if sign(u2)==sign(u1)
   i_fim=i_fim-1; u2=A(i_fim,ce);
   while sign(A(i_fim-1,ce))~=sign(u1)
   i_fim=i_fim-1;
   u2=A(i_fim,ce);
   end
end         % ao sair (i_fim,u2) é um valor apos o ultimo cruzamento.
i_ini=i_ini+1; u1=A(i_ini,ce);

% calcula os instantes de cruzamento de zero por interpolação.
u1m=A(i_ini-1,ce); u2m=A(i_fim-1,ce);
tzero_ini= A(i_ini,2) - Ts*abs(u1)/(abs(u1m)+abs(u1));
tzero_fim= A(i_fim,2) - Ts*abs(u2)/(abs(u2m)+abs(u2));
ttot=tzero_fim - tzero_ini;

% testa do valor de ttot baseado na frequencia
disp('Número de periodos selecionados: deve ser (quase) inteiro! ...'), disp(ttot*f)

%plota com os marcadores para visualizacao
title('Encoder position & markers');
plot(i_ini,A(i_ini,cs),'ko');      
plot(i_fim,A(i_fim,cs),'ko');
figure(3)
plot(A(:,1),A(:,ce)); hold on; grid on;
plot(i_ini,u1,'ko');      
plot(i_fim,u2,'ko');
title('Control effort & markers');
ylabel('Control effort (Volts ou Counts)');
xlabel('amostras');

%calcula o valor DC do sinal de saida
y_raw = A(i_ini:i_fim,cs);
dc_y = Ts*sum(y_raw)/ttot;
disp('Valor DC do sinal de saída ...'), disp(dc_y)
y_t = y_raw -dc_y;

%calcula o valor de pico do sinal da entrada
u_t = A(i_ini:i_fim,ce); % valor DC ja' foi retirado 
Ao= sqrt(2*Ts*sum(u_t.^2)/ttot)

%calculo da integral (soma discreta) a partir do regime.
% definicao do seno e cosseno
t=A(i_ini:i_fim,2);
Sen_omega = sign(u1)*sin(2*pi*f*(t-tzero_ini));
Cos_omega = sign(u1)*cos(2*pi*f*(t-tzero_ini));

saida1=Ts*sum(y_t.*Sen_omega)/ttot;  %resultado da integral com o seno
saida2=Ts*sum(y_t.*Cos_omega)/ttot;  %resultado da integral com o cosseno

%calculo do fi e Bo diretamente das formulas.
fi=atan2(saida2,saida1)*180/pi;
if fi > 0, fi=fi-360; end
Bo=2*sqrt(saida1^2 + saida2^2);
Ar=Bo/Ao;

%mostra as defasagens entre o sinal de saida normalizado, o seno
%e o cosseno utilizados na integral (soma discreta).
figure(4);
plot(t,sign(u1)*y_t/Bo,t,sign(u1)*Sen_omega,t,sign(u1)*Cos_omega); grid on;
title('Defasagem');
xlabel('tempo');


%%%%%%%%%%%% função plota (primeiro gráfico %%%%%%%%%%%%%%%%%%%
function plota(A,ce,cs)
figure(1);
subplot(2,1,1);
plot(A(:,2),A(:,ce));
ylabel('Sinal de Entrada');
xlabel('tempo');

subplot(2,1,2);
plot(A(:,2),A(:,cs));
ylabel('Sinal de Saída (counts)');
xlabel('tempo');