
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                           %
%        Testar Metodos Matemáticos         %
%                                           %
%    Gon�alo Alberto Ramos Carpinteiro      %
%                N� 49642                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Clear all memory and close all figures.
close all; clc

% Abrir ficheiros.

file_f = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Fanuc.txt');
file_s = importdata('/home/carpinteiro/workingcopies/lar4/src/sensors/optoelectric/data/Sensor.txt');

file_f(:,1)=file_f(:,1)-file_s(1,1);
file_s(:,1)=file_s(:,1)-file_s(1,1);

file_s(:,2:end)=file_s(:,2:end)*10;

% Zona de calibração

 figure('position',[100 50 600 600],'color',[1 1 1])
  
  plot(file_f(:,1),file_f(:,4))
  hold on
  des=424+267/2;
  plot(file_s(:,1),file_s(:,2)-des,'r')
  plot(file_s(:,1),file_s(:,3)-des,'g')
  plot(file_s(:,1),file_s(:,4)-des,'k')
  plot(file_s(:,1),file_s(:,5)-des,'y')
  grid on
  title('Altura lida do fanuc no seu referencial global e sensores.')
 xlabel('Tempo decorrido')
 ylabel('Altura')
 %axis([1 4.5 -420 -270])
 legend('Fanuc','A0','A1','A2','A3');

 export_fig('Dados1.pdf','-pdf')
 
 %  % Calibração
%  tets=find(file_s(:,1)<1);
%  in=tets(end);
%  tets=find(file_s(:,1)<3);
%  en=tets(end);
%  
%  % Media dos valores iniciais
%  
%  A0i=[];A1i=[];A2i=[];A3i=[];
%  
%  for n=in:1:en
%      
%      A0i=[A0i file_s(n,2)];
%      A1i=[A1i file_s(n,3)];
%      A2i=[A2i file_s(n,4)];
%      A3i=[A3i file_s(n,5)];
%      
%  end

A0i=mean(A0i); A1i=mean(A1i); A2i=mean(A2i); A3i=mean(A3i);

 
 %%
 
 % Correr os eixos
 run seixos;
 
 % Coordenadas dos sensores
 
 X = [0 -350 350 0];
 Y = [350 0 0 -350];
 Z0 = [40 70 70 40];
 

 figure('position',[100 50 600 600],'color',[1 1 1])
 
 
 % Interpolar para os sensores
 file_sf(:,1)=file_f(:,1);
 file_sf(:,2)=interp1(file_s(:,1),file_s(:,2),file_f(:,1));
 file_sf(end,2)=120.1294;
 file_sf(:,3)=interp1(file_s(:,1),file_s(:,3),file_f(:,1));
 file_sf(end,3)= 195.6307;
 file_sf(:,4)=interp1(file_s(:,1),file_s(:,4),file_f(:,1));
 file_sf(end,4)= 147.5610;
 file_sf(:,5)=interp1(file_s(:,1),file_s(:,5),file_f(:,1));
 file_sf(end,5)= 251.0710;
 % Transformação do fanuc meio do intervalo
 
  figure('position',[100 50 600 600],'color',[1 1 1])
  
  plot(file_f(:,1),file_f(:,4))
  hold on
  des=424+267/2;
  plot(file_sf(:,1),file_sf(:,2)-des,'r')
  plot(file_sf(:,1),file_sf(:,3)-des,'g')
  plot(file_sf(:,1),file_sf(:,4)-des,'k')
  plot(file_sf(:,1),file_sf(:,5)-des,'y')
  grid on
  title('Altura lida do fanuc no seu referencial global e sensores.')
 xlabel('Tempo decorrido')
 ylabel('Altura')
 %axis([1 4.5 -420 -270])
 legend('Fanuc','A0','A1','A2','A3');

 export_fig('Dados2.pdf','-pdf')
 
%%

width=1000;
cx = -80;
cy = -880;
cz = -400;
Zmed2 =[];
Zmed3=[];
 
 for n=1:size(file_sf,1)

 transform = trans3(file_f(n,2),file_f(n,3),file_f(n,4))*RPY([degtorad(file_f(n,7)) degtorad(file_f(n,6)) degtorad(file_f(n,5))]);

A0p=file_sf(n,2);
A1p=file_sf(n,3);
A2p=file_sf(n,4);
A3p=file_sf(n,5);
  
%  subplot(1,2,2)
 
 %Draw axis
 object=transform*P;
 fill3(object(1,:),object(2,:),object(3,:),'b');
 Z=[A0p A1p A2p A3p];
 
 hold on
 
 
 %Draw sensors origins
 Org=[X;Y;Z0;[1 1 1 1]];
 Orgf=transform*Org;
 plot3(Orgf(1,:),Orgf(2,:),Orgf(3,:),'g*');


%Draw sensors position
Pnts=[X;Y;Z;[1 1 1 1]];
Pts=transform*Pnts;
plot3(Pts(1,:),Pts(2,:),Pts(3,:),'r*');

%Draw line
line([Orgf(1,1) Pts(1,1)],[Orgf(2,1) Pts(2,1)],[Orgf(3,1) Pts(3,1)])
line([Orgf(1,2) Pts(1,2)],[Orgf(2,2) Pts(2,2)],[Orgf(3,2) Pts(3,2)])
line([Orgf(1,3) Pts(1,3)],[Orgf(2,3) Pts(2,3)],[Orgf(3,3) Pts(3,3)])
line([Orgf(1,4) Pts(1,4)],[Orgf(2,4) Pts(2,4)],[Orgf(3,4) Pts(3,4)])

% Plano horizontal metodo 2
 Ph=get_plan_meth_2(Pts(1,:),Pts(2,:),Pts(3,:));
 Plan=get_plan(Ph(1),Ph(2),Ph(3),Ph(4));

%Get plan
plot3(Plan(1,:),Plan(2,:),Plan(3,:),'-k')

% Alutra media

Zmed2=[Zmed2 mean(mean(Planf(3,:)))];

% Angulo

a=Ph(1);b=Ph(2);c=Ph(3);d=Ph(4);

 lx=d*c/a;
 ly=d*c/b;
        
 pit2=rad2deg(atan(d/lx));
 rol2=rad2deg(atan(d/ly));

 grid on
 xlabel('x')
 ylabel('y')
 zlabel('z')
 axis([ cx-width/2 cx+width/2 cy-width/2 cy+width/2 cz-width/2 cz+width/2])
   title(sprintf('Horizontal Metodo 2 Zmed: %3.2d pit: %3.2d rol: %3.2d ',mean(mean(Planf(3,:))),rad2deg(atan(d/lx)),rad2deg(atan(d/ly))));
 hold off
 
 
 pause(0.005)
 
end
 %%
 % Plano horizontal metodo 3
 
 Z=Z0+[A0p A1p A2p A3p];
 
 Ph=get_plan_meth_3(X,Y,Z);
 Plan=get_plan(Ph(1),Ph(2),Ph(3),Ph(4));
 
 subplot(1,2,1)
 
 %Draw axis
 object=transform*P;
 fill3(object(1,:),object(2,:),object(3,:),'b');
 
 
 hold on
 

 
 %Draw sensors origins
 Org=[X;Y;Z0;[1 1 1 1]];
 Orgf=transform*Org;
 plot3(Orgf(1,:),Orgf(2,:),Orgf(3,:),'g*');


%Draw sensors position
Pnts=[X;Y;Z0;[1 1 1 1]];
Pnts(3,:)=Z;
Pts=transform*Pnts;
plot3(Pts(1,:),Pts(2,:),Pts(3,:),'r*');

%Draw line
line([Orgf(1,1) Pts(1,1)],[Orgf(2,1) Pts(2,1)],[Orgf(3,1) Pts(3,1)])
line([Orgf(1,2) Pts(1,2)],[Orgf(2,2) Pts(2,2)],[Orgf(3,2) Pts(3,2)])
line([Orgf(1,3) Pts(1,3)],[Orgf(2,3) Pts(2,3)],[Orgf(3,3) Pts(3,3)])
line([Orgf(1,4) Pts(1,4)],[Orgf(2,4) Pts(2,4)],[Orgf(3,4) Pts(3,4)])

%Get plan

Planf=transform*Plan;
plot3(Planf(1,:),Planf(2,:),Planf(3,:),'-k')

Zmed3= [Zmed3 mean(mean(Planf(3,:)))];


% Angulo

a=Ph(1);b=Ph(2);c=Ph(3);d=Ph(4);

 lx=d*c/a;
 ly=d*c/b;
        
 pit2=rad2deg(atan(d/lx));
 rol2=rad2deg(atan(d/ly));

 grid on
 xlabel('x')
 ylabel('y')
 zlabel('z')
 axis([ cx-width/2 cx+width/2 cy-width/2 cy+width/2 cz-width/2 cz+width/2])
  title(sprintf('Horizontal Metodo 3 Zmed: %3.2d pit: %3.2d rol: %3.2d ',mean(mean(Planf(3,:))),rad2deg(atan(d/lx)),rad2deg(atan(d/ly))));
 hold off
 
 pause(0.005)
 
end
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 