%--------------------------------------------------------------------------------------
% Ficheiro:		Sim_PanTilt.m
%
% Autor: 		Filipe Silva
% Data:			17-Jan-2008
% Modificado:   13-Fev-2008 
% Objectivo:	Simulaçao/controlo do movimento de PAN&TILT
%--------------------------------------------------------------------------------------

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) ------
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Representação gráfica do sistema na configuração inicial: POSIÇAO ZERO 
%--------------------------------------------------------------------------------------
qi = [0 pi/4]; %-- Postura vertical ---------------------------------------------------
B = BallCoordDK( qi,L );  %-- Coordenadas da bola -------------------------------------
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

%--------------------------------------------------------------------------------------
% Espaço de trabalho da bola  
%--------------------------------------------------------------------------------------
N = 20;
q1 = linspace(-pi/2,pi/2,N); %-- Junta 1 ----------------------------------------------
q2 = linspace(pi/10,pi/2,N);    %-- Junta 2 -------------------------------------------
[QQ1,QQ2] = meshgrid(q1,q2);
k = 1;
for i = 1:N,
    for j = 1:N,      
        XYb = BallCoordDK( [QQ1(i,j) QQ2(i,j)],L );  %-- Coordenadas da bola ------------
        B(:,k) = XYb;
        k = k+1;
    end
end
%-- Coordenadas da bola ---------------------------------------------------------------
DesenharRobo( P,T,H,C,B ), pause
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% MOVETYPE: "FreeStyle"
%--------------------------------------------------------------------------------------            
 qf = [ -pi/2   pi/4    ;
         pi/2   pi/4    ;
         0      pi/4    ;
         0      pi/2    ;
         0      pi/4    ;
         0      pi/10  ];
%-----------------------------------------------------------------------------------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 ---------------------------------
 Tsim = 0;                 %-- Tempo de simulaçao (inicializaçao) -------------------
 for k=1:size(qf,1),       %-- Movimento dividido em "i" fases ----------------------
      
 %__ Matriz tempo de simulaçao ------------------------------------------------------
  t(k,:) = tp+Tsim; Tsim = Tsim+Texe;
 %__ Velocidade e acelaraçao inicial e final ----------------------------------------
  for n=1:length(qi), VEL(n,:) = [0 0]; ACE(n,:) = [0 0]; end 
  [Q_head,dQ_head,ddQ_head] = PlaneamentoJuntas( tp,qi,qf(k,:),VEL,ACE );     
 %__ Vector das juntas --------------------------------------------------------------
  Q(:,:,k) = Q_head;
 %__ Reinicializaçao do vector das juntas -------------------------------------------
  qi = qf(k,:);   
        
 end
 % size(Q), pause
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Coordenadas Cartesianas do sistema completo
%--------------------------------------------------------------------------------------
 for i=1:size(Q,3), %-- Movimento dividido em "i" fases -----------------------------
 for k=1:size(Q,2), %-- ... ao longo do tempo "k" -----------------------------------

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