%--------------------------------------------------------------------------------------
% Ficheiro:		SIM_LocateAlign.m
%
% Autor: 		Filipe Silva
% Data:			17-Jan-2008
% Modificado:   13-Fev-2008 
% Objectivo:	Simulaçao/controlo do movimento de PAN&TILT baseada em realimentaçao 
%               visual para tarefas de "searching" e "tracking" de bola no solo
%--------------------------------------------------------------------------------------

clear all
close all
clc

%--------------------------------------------------------------------------------------
% Inicializaçao de constantes e variaveis
%--------------------------------------------------------------------------------------
 %__ Parametros da PAN&TILT -----------------------------------------------------------
 l0 = 0.023; l1 = 0.0147; l2 = 0.02; l3 = 0.05; l4 = 0.04;
 %-- Secçao do tronco (l,w,h): comprimento x largura x altura --------------------------
 lt_L = 0.1940;  lt_W = 0.072;  lt_H = 0.098;
 %-- Vector comprimentos ---------------------------------------------------------------
 L = [l0 l1 l2 l3 l4 lt_L lt_W lt_H];
 %__ Outras constantes ---------------------------------------------------------------- 
 DIM = 24;                      %-- Dimensão do vector tempo de simulaçao (Tsim) ------
 %__ Parametros da camara: res. 256x256, dist_focal = 4e-3, pixel_pitch = 30e3 pix/m
 %__                                                        => DIM_ccd = 8.5x8.5 mm 
 dfocal = 4e-3;
 pix_pitch = 30e3;
 max_Pccd = 128/pix_pitch;
%-----------------------------------------------------------------------------------end

%######################################################################################
% PARAMTEROS DE ENTRADA PARA A SIMULAÇAO: search the ball and align the camera        #
%######################################################################################
 qi = [ -pi/4  pi/4 ];     %-- Posiçao angular inicial --------------------------------
 Pb = [ 0.05 0.2  0.1 0.3 0.05 0.5
        0.05 0.2 -0.2 0   0    0  ];   %-- Posiçao real da bola -----------------------
%-----------------------------------------------------------------------------------end

for NN = 1:size(Pb,2),
   
%--------------------------------------------------------------------------------------
% Representação gráfica do sistema na configuração NN
%--------------------------------------------------------------------------------------
 B  = [ Pb(1,NN) ;  Pb(2,NN) ]  %-- Posiçao real da bola ------------------------------
 Bt = [B BallCoordDK( qi,L )];    %-- + posiçao da bola se alinhada com camara ... ----
 Ai = ParamDH( qi,L );
 [P,T,H,C] = RobCoord( Ai,L ); % size(P), size(T), size(H), size(C)
 DesenharRobo( P,T,H,C,Bt ), pause
%-----------------------------------------------------------------------------------end

%######################################################################################
% SIMULAÇAO DO PROCESSO DE AQUISIÇAO: projecçao da bola no plano de imagem
%######################################################################################
 rTc = Ai(:,:,1)*Ai(:,:,2)*Ai(:,:,3)*Ai(:,:,4)*Ai(:,:,5); %############################
 [Cx,Cy] = ImageBallCM( rTc,B,dfocal ); %##############################################
% figure(2), plot(Cx,Cy,'b*') %#########################################################
% xlabel('X'), ylabel('Y'), title('Plano de Imagem'), axis([-0.05 0.05 -0.05 0.05])
%###################################################################################### 
% ESTIMAR a posiçao da bola no sistema R com base nas coordenadas da imagem
% ... sabendo que a bola se encontra no plano horizontal (para obter Cz)
%--------------------------------------------------------------------------------------
 B_estimated = RefCoordBallCM( qi,rTc,Cx,Cy,dfocal,L )
%-----------------------------------------------------------------------------------end 

%--------------------------------------------------------------------------------------
% Cinematica inversa: q = f(xb,yb)
%--------------------------------------------------------------------------------------
 qf = BallCoordIK( B_estimated,L ); %-- Posiçao angular final -------------------------
%-----------------------------------------------------------------------------------end 

%--------------------------------------------------------------------------------------
% Planeamento de trajectorias nas juntas (funçoes polinomiais)
%--------------------------------------------------------------------------------------
 Texe = 1.0; Ts = Texe/25; %-- Periodo de amostragem e tempo de execuçao ------------
 tp = 0:Ts:Texe;           %-- Vector tempo virtual ---------------------------------
 %__ Velocidade e acelaraçao inicial e final ----------------------------------------
  for n=1:length(qi), VEL(n,:) = [0 0]; ACE(n,:) = [0 0]; end 
  [Q,dQ,ddQ] = PlaneamentoJuntas( tp,qi,qf,VEL,ACE );     
 % size(Q), pause
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Coordenadas Cartesianas do sistema completo
%--------------------------------------------------------------------------------------
 for k=1:size(Q,2), %-- ... ao longo do tempo "k" -----------------------------------

     Bt = [B BallCoordDK( Q(:,k),L )];
     Ai = ParamDH( Q(:,k),L );
     [P,T,H,C] = RobCoord( Ai,L ); % size(P), size(B), size(H), size(C)
     DesenharRobo( P,T,H,C,Bt ), pause(0.05) 
     
 end
%-----------------------------------------------------------------------------------end

 qi = qf;
 
end %----------------------------------------------------------------------------endFOR

pause, close all, return
