%--------------------------------------------------------------------------------------
% Ficheiro:		Sim_OneLeg.m
%
% Autor: 		Filipe Silva
% Data:			24-Maio-2006
% Objectivo:	Simulação cinemática de uma perna 2 dof em equilibrio num plano movel
%--------------------------------------------------------------------------------------

clear all
clc

%--------------------------------------------------------------------------------------
% Aproximaçoes das funçoes SIN e COS usando a serie de TAYLOR
%--------------------------------------------------------------------------------------
if 0,
    
 teta = 0:pi/100:pi/2;
 SIN_mat = sin(teta);           %-- sin_matlab ---------------------------------------- 
 COS_mat = cos(teta);           %-- cos_matlab ----------------------------------------
%-- SIN&COS_app(teta,c,n) -------------------------------------------------------------
 c1 = 22.5*pi/180;
 c2 = 67.5*pi/180;
 n  = 3;     %-- Numero de termos n                 ------------------------------------ 
 for i=1:length(teta),
     if teta(i) < 45*pi/180, SIN_num(i) = SIN_app(teta(i),c1,n); 
                             COS_num(i) = COS_app(teta(i),c1,n);
     else
                             SIN_num(i) = SIN_app(teta(i),c2,n); 
                             COS_num(i) = COS_app(teta(i),c2,n);
     end
 end
%-- Funçao SIN ------------------------------------------------------------------------ 
 plot( teta*180/pi,SIN_mat,'k-.', teta*180/pi,SIN_num,'r',...
       teta*180/pi,SIN_app(teta,c1,n),'b-.', teta*180/pi,SIN_app(teta,c2,n),'g-.' )
 xlabel('teta (º)'), ylabel('SIN'), pause
 
 err_SIN = abs(SIN_mat-SIN_num); I = find(err_SIN == max(err_SIN));
 err_max_SIN = [max(err_SIN) SIN_mat(I) SIN_num(I)]
 plot( teta*180/pi,SIN_mat-SIN_num,'r',...
       teta*180/pi,SIN_mat-SIN_app(teta,c1,n),'b-.', teta*180/pi,SIN_mat-SIN_app(teta,c2,n),'g-.' )
 xlabel('teta (º)'), ylabel('err_SIN'), pause
%-- Funçao COS ------------------------------------------------------------------------  
 plot(teta*180/pi,COS_mat,'k-.', teta*180/pi,COS_num,'r',...
      teta*180/pi,COS_app(teta,c1,n),'b-.', teta*180/pi,COS_app(teta,c2,n),'g-.' )
 xlabel('teta (º)'), ylabel('COS'), pause

 err_COS = abs(COS_mat-COS_num); I = find(err_COS == max(err_COS));
 err_max_COS = [max(err_COS) COS_mat(I) COS_num(I)]
 plot( teta*180/pi,COS_mat-COS_num,'r',...
       teta*180/pi,COS_mat-COS_app(teta,c1,n),'b-.', teta*180/pi,COS_mat-COS_app(teta,c2,n),'g-.' )
 xlabel('teta (º)'), ylabel('err_COS'), pause
end%***********************************************************************************

%--------------------------------------------------------------------------------------
% Definição de constantes e OPÇOES
%--------------------------------------------------------------------------------------
%-- Estrutura da perna ----------------------------------------------------------------
 l0=0.2;                        %-- Comprimento do pe de massa desprezavel ------------
 l1=0.5;	 r1=l1/2; m1=0.5;   %-- Comprimento e massa do elo tornozelo-joelho -------
 l2=0.5;	 r2=l2/2; m2=0.5;	%-- Comprimento e massa do elo joelho-anca ------------
 L = [l1 ; l2]; R = [r1 ; r2]; M = [m1 ; m2];
%-- Plano movel -----------------------------------------------------------------------
 A0 = pi/6; T = 2; Ts = 5e-3;
%--------------------------------------------------------------------------------------
% Ciclo infinito: descrição dos requisitos da tarefa operacional
%--------------------------------------------------------------------------------------
%-- Simulação temporal ----------------------------------------------------------------
 Tsim = T;               %-- Tempo de execução do movimento ------------------------
 t	 = 0:Ts:5*Tsim;      %-- Vector tempo (virtual)         ------------------------
 DIM  = length(t),	     %-- Dimensão do vector tempo       ------------------------
 %-- Movimento do plano ------------------------------------------------------------
 q0 = A0*sin(2*pi*t/T); q1 = 80*pi/180; q2 = 40*pi/180; %-- Postura inicial --------
 Q = [q0(1); q1; q2];
 P  = CinDir2R( Q,[l0; L] );      %-- Cinematica directa "completa" ---------
 COG = RobCOG( Q,L,R,M );      %-- Centro de gravidade -------------------
 DesenharRobo( P, COG )
 disp('Prima uma tecla para começar ...'), pause

%--------------------------------------------------------------------------------------
% Simulaçao cinematica da perna em equilibrio num plano em movimento
%--------------------------------------------------------------------------------------
APP = 1; n = 3;    %-- APP= 0: SIN&COS do Matlab; 1: aproximaçao polinomial (n termos)
MOD = 2;    %-- MOD= 0: (CoGx,CoGy) -> 2dof; 1: (CoGx,0) -> 2dof; 2: (CoGx,0) -> 1dof
for i=2:DIM,
%--------------------------------------------------------------------------------------
% Medir sensores de força para "sentir" o movimento da plataforma: COG = COP 
%--------------------------------------------------------------------------------------
 COG_med = RobCOG( [q0(i);Q(2,i-1);Q(3,i-1)],L,R,M ); %-- "Sentir" plataforma: ang0
%--Acçao de controlo: dq = J(CoG)^T*(CoG_ref - CoG_sen) -------------------------------
 if APP == 0,
    s1 = sin(Q(2,i-1));                     s12 = sin(Q(2,i-1)+Q(3,i-1));
    c1 = cos(Q(2,i-1));                     c12 = cos(Q(2,i-1)+Q(3,i-1));
 else                                      
     q_1 = abs(Q(2,i-1));                   c_1 = PARAM_app( q_1 );    
     s1  = SIN_app( q_1,c_1,n);             c1  = COS_app( q_1,c_1,n );

     q_12 = abs(Q(2,i-1)+Q(3,i-1));         c_12 = PARAM_app( q_12 );
     s12  = SIN_app( q_12,c_12,n );         c12  = COS_app( q_12,c_12,n );
 end                                      
 JT = [ -m1*r1*s1-m2*(l1*s1+r2*s12)         m1*r1*c1+m2*(l1*c1+r2*c12)
        -m2*r2*s12                          m2*r2*c12                  ]/(m1+m2);
 switch MOD,
     case 0, Kp=200; dQ     = Kp*JT*(COG(:,1)-COG_med);  
                     Q(:,i) = [ q0(i); Q(2,i-1)+Ts*dQ(1); Q(3,i-1)+Ts*dQ(2) ];        
                     
     case 1, Kp=200; dQ     = Kp*JT*[COG(1,1)-COG_med(1) ; 0];
                     Q(:,i) = [ q0(i); Q(2,i-1)+Ts*dQ(1); Q(3,i-1)+Ts*dQ(2) ];                 
                     
     case 2, Kp=200; dQ = Kp*JT(1,:)*[COG(1,1)-COG_med(1) ; 0];
                     Q(:,i) = [ q0(i); Q(2,i-1)+Ts*dQ(1); Q(3,i-1) ];                 
 end 
 COG(:,i) = RobCOG( Q(:,i),L,R,M );
 P(:,i)   = CinDir2R( Q(:,i),[l0; L] ); 
end%***********************************************************************************

%--------------------------------------------------------------------------------------
% Visualizaçao de dados: COG(t), Q(t)
%--------------------------------------------------------------------------------------
% figure(2), plot(t,COG(1,:),'r', t,COG(2,:),'b')
% xlabel('Tempo (s)'), ylabel('CoG_x&y (m)'), grid on, pause
 figure(2), plot(t,Q(1,:)*180/pi,'k-.', t,Q(2,:)*180/pi,'r', t,Q(3,:)*180/pi,'b')
 xlabel('Tempo (s)'), ylabel('Q (º)'), pause
% figure(2), plot(t,P(14,:),'r'), xlabel('Tempo (s)'), ylabel('Y_h_i_p (m)'), grid on
% pause
%***********************************************************************************END

%--------------------------------------------------------------------------------------
% Animaçao do comportamento da perna
%--------------------------------------------------------------------------------------
 DesenharRobo( P, COG )
%%%%*******************************************************************************END*

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