%--------------------------------------------------------------------------------------
% Ficheiro:		SimWalking.m
%
% Autor: 		Filipe Silva
% Data:			31Maio2004
% Objectivo:	Animação cinemática do movimento do robo bipede 12dof-3D
%--------------------------------------------------------------------------------------

clear all
close all
clc
%warning off

%--------------------------------------------------------------------------------------
% Definição de CONSTANTES & OPÇOES
%--------------------------------------------------------------------------------------
 go=9.8;					    %-- Aceleração da gravidade ---------------------------
%-- Estrutura do braço manipulador ----------------------------------------------------
 l0 =0.02;	 r0=l0/2; m0=0.28;  %-- Comprimento e massa do elo pe-tornozelo -----------
 l1 =0.022;	 r1=l1/2; m1=0.20;  %-- Comprimento e massa do elo tornozelo-tornozelo ----
 l2 =0.16;	 r2=l2/2; m2=0.63;	%-- Comprimento e massa do elo tornozelo-joelho -------
 l3 =0.12;   r3=l3/2; m3=0.26;	%-- Comprimento e massa do elo joelho-anca ------------
 l4 =0.053;  r4=l4/2; m4=0.14;	%-- Comprimento e massa do elo anca-anca --------------
 l5 =0.018;  r5=l5/2; m5=0.07;	%-- Comprimento e massa do elo anca-anca --------------
 l5h=0.050;                 	%-- Comprimento/altura e massa da secçao da anca ------
 l5t=0.125;                  	%-- Comprimento e massa do elo anca-tronco ------------
 l6 =0.15;   r6=l6/2; m6=5.5;   %-- Comprimento/largura da secçao da anca -------------
 L=[l0 l1 l2 l3 l4 l5 l5h l5t l6];   M=[m0 m1 m2 m3 m4 m5 m6];
 R=[r0 r1 r2 r3 r4 r5];  

%-- Definiçoes sobre o ciclo de locomoçao ---------------------------------------------
 DIM = 24;                      %-- Dimensão do vector tempo -------------------------- 
 SS = 1; DS = 0;                %-- Definiçao das fases de SS e DS --------------------
 RL = 1; LL = 0;                %-- Perna de referencia -------------------------------
 XYRef = [0 0];                 %-- Coordenadas de referencia -------------------------
%------------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Representação gráfica do robô na configuração inicial: POSIÇAO ZERO 
%		1) Função CinDir2R: cálculo das coordenadas Cartesianas do robô RR
%		2) Script DesenharRobo: plot dos elos mecânicos, juntas e músculos
%--------------------------------------------------------------------------------------
qi = [0 0 0 0 0 0 0 0 0 0 0 0];
Ai = CinDir12dof( qi,L,XYRef ); 
[P,C,F] = CoordJuntas( Ai,XYRef );
COG = RobCOG( qi,L,R,M );
DesenharRoboPZ( P,C,F )
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Ciclo de movimentos PtP: descrição dos requisitos da tarefa operacional
%--------------------------------------------------------------------------------------        
%-- Parametros de locomoçao -----------------------------------------------------------
 Sl = 0.10; Vf  = 0.1;          %-- Comprimento do passo e Velocidade de Progressao ---
 Hh = 0.39; Fc = 0.02;          %-- Altura da anca e Elevaçao do Pe -------------------
 Yg = 0.0;                      %-- Posiçao lateral do centro da anca -----------------
%-- Especificaçao do movimento: vector P(xi,yi,zi) ------------------------------------
FROM   = [  P(9,1)  P(9,2)  P(9,3)-l5h/2  P(18,1)  P(18,2)  P(18,3)  P(18,3)  DS  1 ];
GAIT   = 2;
switch GAIT,
 %-- Coordenadas da anca -> (xh,yh,zh) e pe livre -> (xf,yf,zf) -----------------------
 % MOVETO= [  xh      yh      zh            xf       yf       zf       Fc      Sup GAIT
 %-------------------------------------------------------------------------------------
  case 1,
  MOVETO = [ -Sl/2    Yg      Hh           -Sl       NaN      0.0      0.0      SS  1
              Sl/2    Yg      Hh            Sl       NaN      0.0      Fc       SS  1
              Sl/2    l6/2    Hh            Sl       NaN      0.0      0.0      DS  1 ];
  case 2,       
  MOVETO = [ -Sl/4    Yg      Hh           -Sl       NaN      0.0      0.0      SS  2
              Sl/4    Yg      Hh            Sl       NaN      0.0      Fc       SS  2
            3*Sl/4    l6-Yg   Hh            Sl       NaN      0.0      0.0      DS  2 ];          
end
%-- Simulaçao do movimento: 1 PASSO ---------------------------------------------------
for k=1:size(MOVETO,1),
   %-- Escolha dos movimentos e realizar ----------------------------------------------
    MOVE_OnOff = [ 1 1 1 ];
    if MOVE_OnOff(k) == 0, continue, end
    BuildMotion;                    %-- obter angulos das juntas (cinematica inversa) 
    FROM = MOVETO(k,:);  qi = qf;   %-- reinicializaçao de variaveis
end

%--------------------------------------------------------------------------------------
% Evolução temporal das juntas ao longo de 1/2 ciclo -> 1 passo (movimento simétrico!)
%--------------------------------------------------------------------------------------
 PlotJuntas( t,Q )
 disp('Prima uma tecla para iniciar animação ...'), pause
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Animaçao do andar bipede
%--------------------------------------------------------------------------------------
 Animacao3D( Q,L,R,M, XYRef,Sl,Yg,GAIT )
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Sair
%--------------------------------------------------------------------------------------
 close all, return
%--------------------------------------------------------------------------
%---------end