%--------------------------------------------------------------------------------------
% Ficheiro:		Sim_OneLeg.m
%
% Autor: 		Filipe Silva
% Data:			29Nov2003
% Objectivo:	Modelização cinemática e dinâmica de uma perna 3 dof (planar)
%--------------------------------------------------------------------------------------

clear all
clc

%--------------------------------------------------------------------------------------
% Definição de constantes
%--------------------------------------------------------------------------------------
go=9.8;					%-- Aceleração da gravidade -----------------------------------

%-- Estrutura do braço manipulador ----------------------------------------------------
l1=0.16;	 r1=l1/2; m1=0.75;   %-- Comprimento e massa do elo tornozelo-joelho -----------
l2=0.16;	 r2=l2/2; m2=0.75;	%-- Comprimento e massa do elo joelho-anca ----------------
l3=0.1;      r3=l3;   m3=5.5;	%-- Comprimento e massa da carga colocada no tronco -------

%-- Simulação temporal ----------------------------------------------------------------
Ts		= 1.0e-3;		%-- Período de amostragem          ----------------------------
h		= Ts;				%-- Período de simulação           ------------------------
Tsim	= 2;				%-- Tempo de execução do movimento ------------------------
t		= 0:Ts:Tsim;	%-- Vector tempo (virtual)         ----------------------------
DIM   = length(t);	%-- Dimensão do vector tempo       --------------------------------

%--------------------------------------------------------------------------------------
% Representação gráfica do robô na configuração inicial: 
%		1) Função CinDir2R: cálculo das coordenadas Cartesianas do robô RR
%		2) Script DesenharRobo: plot dos elos mecânicos, juntas e músculos
%--------------------------------------------------------------------------------------
ang1i = pi/2; ang2i = 0; ang3i = 0; 
q = [ang1i; ang2i; ang3i];	%-- Deslocamento angular inicial ---
P = CinDir3R(ang1i,ang2i,ang3i,l1,l2,l3);
[COG_1, COG_2, COG_3, COG] = RobCOG( q,l1,l2,l3,r1,r2,r3,m1,m2,m3 );             %*
DesenharRobo( P, COG, 0 )

%--------------------------------------------------------------------------------------
% Ciclo infinito: descrição dos requisitos da tarefa operacional
%--------------------------------------------------------------------------------------
CONTINUAR = 1;
Xi = P(5,1); Yi = P(6,1);		%-- Xi=l2 & Yi=l1 ---------------------------------------
while CONTINUAR == 1,
   
    [Xf,Yf] = ginput(1)
    if (isempty(Xf) | isempty(Yf)), CONTINUAR = 0; break, end 
    if ( Xf^2+Yf^2 > (l1+l2)^2 ) continue, end
%%%% ESPAÇO DAS JUNTAS ****************************************************************
   %----------------------------------------------------------------------------------*
   % Geração de trajectórias usando interpolação polinomial                           *
   % Planeamento de trajectórias no espaço Cartesiano                                 *
   %----------------------------------------------------------------------------------*
    [trajX, dtrajX, ddtrajX] = GeradorTraj(t, [Xi Xf]);	%-- Componente X ---------*
    [trajY, dtrajY, ddtrajY] = GeradorTraj(t, [Yi Yf]);	%-- Componente Y ---------*
    dtraj = [dtrajX; dtrajY];                                                        %*
    [ang_t, dang_t, ddang_t] = GeradorTraj(t, [pi/2 pi/2]);	%-- Angulo de inclinaçao -*
   %----------------------------------------------------------------------------------*
   % Simulação do movimento do robô ao longo de uma linha recta                       *
   %[q, dq] = MovContinuo(t,q(:,1),dtraj); -> usando a d/dt                           *
   %----------------------------------------------------------------------------------*
    [q1,q2,q3] = CinInv3R(trajX,trajY,ang_t,l1,l2,l3); %- Deslocamento angular -------*
    [dq1,dq2,dq3,ddq1,ddq2,ddq3]=CinInvDif3R(trajX,trajY,dtrajX,dtrajY,...
                                             ddtrajX,ddtrajY,l1,l2,l3);
    q = [q1; q2; q3]; dq = [dq1; dq2; dq3]; ddq = [ddq1; ddq2; ddq3];                %*
 size(q), size(dq), size(ddq), pause                                                                                     %* 
   %----------------------------------------------------------------------------------*
   % Dinâmica Inversa e PlotGrafs                                                     *
   %----------------------------------------------------------------------------------*
    [J, C, G, T] = DinInv3R(q,dq,ddq,l1,l2,l3,r1,r2,r3,m1,m2,m3,go);                 %*
    PlotJuntasD			%-- Evolução temporal das posições, velocidades e acelerações *
    PlotBinD				%-- Evolução temporal dos binários                        *
    pause, close(2), close(3), close(4)
%%%%*******************************************************************************END*

   %----------------------------------------------------------------------------------*
   % Centro de gravidade (COG), centro de pressao (COP) e força de reacçao R=[Rt;Rn]  *
   %----------------------------------------------------------------------------------*
    [COG_1, COG_2, COG_3, COG] = RobCOG( q,l1,l2,l3,r1,r2,r3,m1,m2,m3 );             %*
    R = RobGRF( q,dq,ddq,l1,l2,l3,r1,r2,r3,m1,m2,m3,go );                            %*                             
    COP = RobCOP( T(1,:),R(2,:) );                                                   %*
%%%%*******************************************************************************END*

   %----------------------------------------------------------------------------------*
   % Animação do movimento real do robô (controlo PD)                                 *
   %----------------------------------------------------------------------------------*
    P = CinDir3R(q(1,:),q(2,:),q(3,:),l1,l2,l3);                                     %*
    DesenharRobo( P, COG, COP )                                                   %*
%%%%*******************************************************************************END*
    
   %-----------------------------------------------------------------------------------
   % Actualizar configuração inicial
   %-----------------------------------------------------------------------------------
    LAST_P = P(:,DIM); LAST_q = q(:,DIM);
    clear P q dq
    Xi     = LAST_P(5);		Yi     = LAST_P(6);

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