%--------------------------------------------------------------------------------------
% Ficheiro:		SIM_Tracking.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 = 500;                    %-- 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: ball tracking                               #
%######################################################################################
 xb_i = 0.2;   yb_i =  0.2;  
 xb_f = 0.2;   yb_f = -0.2; 
 MOVETYPE = 0; %-- 0 linear; 1 sinusoidal ---------------------------------------------
 CTRLTYPE = 1 ; %-- 0 image-based control; 1 position based control
%###################################################################################### 
 qi = BallCoordIK( [xb_i yb_i],L ); %-- Posiçao angular inicial -----------------------
 Q = qi;  Qref = qi;
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Planeamento da trajectoria a efectuar pela bola (funçoes polinomiais)
%--------------------------------------------------------------------------------------
 Texe = 5.0; Ts = Texe/DIM; %-- Periodo de amostragem e tempo de execuçao ------------
 tp = 0:Ts:Texe;           %-- Vector tempo virtual ---------------------------------

 switch MOVETYPE,
     case 0, [xb,dxb,ddxb] = PlanLin( tp,xb_i,xb_f,[0 0],[0 0] );     
             [yb,dyb,ddyb] = PlanLin( tp,yb_i,yb_f,[0 0],[0 0] );         
             
     case 1, [xb,yb] = PlanSin( tp,xb_i,yb_i,xb_f,yb_f,0.1,2 );
 end
%-----------------------------------------------------------------------------------end
 
%--------------------------------------------------------------------------------------
% Representação gráfica do sistema na configuração inicial: TT = 1
%--------------------------------------------------------------------------------------
 B  = [xb_i ; yb_i];    %-- Coordenadas iniciais da bola ... 
 B_estimated = B;       %-- Coordenadas estimadas a partir da imagem ...
 Ai = ParamDH( qi,L );
 [P,T,H,C] = RobCoord( Ai,L ); % size(P), size(T), size(H), size(C)
 DesenharRobo( P,T,H,C,B ), pause
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% TAREFA: BallTracking; CONTROLO: Realimentaçao Visual "Image_Based"
%--------------------------------------------------------------------------------------
for TT = 2:length(tp),
    
%######################################################################################
% SIMULAÇAO DO PROCESSO DE AQUISIÇAO: projecçao da bola no plano de imagem
%######################################################################################
 B = [xb(TT) ; yb(TT)];    %-- Coordenadas actuais da bola ############################
 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(:,TT) = RefCoordBallCM( qi,rTc,Cx,Cy,dfocal,L );
%-----------------------------------------------------------------------------------end 

%--------------------------------------------------------------------------------------
% Coordenadas Cartesianas do sistema completo
%--------------------------------------------------------------------------------------
 switch CTRLTYPE,
    
  case 0, %----------------------------------------------------------------------------
          % IMAGE-BASED CONTROL
          % Controlo independente dos eixos: dq1 = k1*(0-Cy) & dq2 = k2*(0-Cx)  
          %----------------------------------------------------------------------------
           k1 = 5000; dq1 = k1*(0-Cx);      Q(1,TT) = Q(1,TT-1)+Ts*dq1; 
           k2 = 5000; dq2 = k2*(0-(-Cy));   Q(2,TT) = Q(2,TT-1)+Ts*dq2;
          %-- Configuraçao ideal das juntas para alinhar camara com bola --------------
           Qref(:,TT) = BallCoordIK( B,L ); %-- Posiçao angular inicial ---------------
  case 1, %----------------------------------------------------------------------------
          % POSITION-BASED CONTROL
          % Controlo baseado no inverso do Jacobiano
          %----------------------------------------------------------------------------
           k1 = 100; k2 = 100; kp = [ k1 0 ; 0 k2 ]; %-- Controlo proporcional
           
           J = Jacobian( Q(:,TT-1),L );     %-- Jacobiano d_q -> d_Rb
           dq = inv(J)*k1*( B_estimated(:,TT)-B_estimated(:,TT-1) );
           Q(1,TT) = Q(1,TT-1)+Ts*dq(1); 
           Q(2,TT) = Q(2,TT-1)+Ts*dq(2);
          %-- Configuraçao ideal das juntas para alinhar camara com bola --------------
           Qref(:,TT) = BallCoordIK( B,L ); %-- Posiçao angular inicial ---------------
           
 end
%-----------------------------------------------------------------------------------end 

%--------------------------------------------------------------------------------------
% Coordenadas Cartesianas do sistema completo
%--------------------------------------------------------------------------------------
 Bt = [ B [xb_f ; yb_f] ];
 Ai = ParamDH( Q(:,TT),L );
 [P,T,H,C] = RobCoord( Ai,L ); % size(P), size(B), size(H), size(C)
 DesenharRobo( P,T,H,C,Bt ), pause(0.001) 
%-----------------------------------------------------------------------------------end  
 
end %----------------------------------------------------------------------------endFOR

%--------------------------------------------------------------------------------------
% Visualizaçao da evoluçao temporal nas juntas
%--------------------------------------------------------------------------------------
 subplot(2,1,1), plot(tp,Q(1,:)*180/pi,'k', tp,Qref(1,:)*180/pi,'b-.'), grid on, legend('Q1','Qref')
 subplot(2,1,2), plot(tp,Q(2,:)*180/pi,'k', tp,Qref(2,:)*180/pi,'b-.'), grid on, legend('Q2','Qref')
%-----------------------------------------------------------------------------------end 
pause, close all, return
