%--------------------------------------------------------------------------------------
% Ficheiro:		MUSCLES4L.m
%
% Autor: 		Filipe Silva
% Data:			15Nov2001
% Objectivo:	Modelização cinemática e dinâmica do manipulador RR:
%					1) Actuado por motores rotacionais
%					2) Actuado por músculos artificiais
%--------------------------------------------------------------------------------------

clear all
clc

%--------------------------------------------------------------------------------------
% Definição de constantes
%--------------------------------------------------------------------------------------
global m1 m2 m3 l1 l2 R Ts h	go %-- Variáveis globais --------------------------------

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

%-- Estrutura do braço manipulador ----------------------------------------------------
l1=35;	
m1=.347;		%-- Comprimento e massa do elo ombro-cotovelo (l1, m1)   ---------
l2=52;%(27)	
m2=.744;		%-- Comprimento e massa do elo cotovelo-punho (l2, m2)   ---------
l3=0.0; 
m3=0.2;		%-- Comprimento e massa da carga colocada na extremidade ---------

%-- Roldana e braços das alavancas ----------------------------------------------------
R   = 0.2;				%-- raio: parâmetro característico da roldana --------------------
q01 = pi/6;				%-- origem do músculos flexor do ombro        --------------------
q02 = -5*pi/6;			%-- origem do músculos extensor do ombro      --------------------
q0  = [q01; q02];

b1  = 0.4*l1; 			%-- b1: flexor e extensor do ombro                     -----------
b21 = 0.2; b22 = 0.1;%-- b21: flexor do cotovelo; b22: extensor do cotovelo -----------
b   = [b1  b1; b21  b22];

%-- Simulação temporal ----------------------------------------------------------------
Ts		= 0.1e-3;		%-- Período de amostragem          -------------------------------
h		= Ts;				%-- Período de simulação           -------------------------------
Tsim	= 1;				%-- 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 =  pi/2;	q = [ang1i; ang2i];	%-- Deslocamento angular inicial ---
P = CinDir2R(ang1i,ang2i);
DesenharRobo( P, q, q0, b )

%--------------------------------------------------------------------------------------
% Ciclo infinito: descrição dos requisitos da tarefa operacional
%--------------------------------------------------------------------------------------

Xi = P(3,1); Yi = P(4,1);


while 1,
    

   %-----------------------------------------------------------------------------------
   % Coordenada inicial (Xi,Yi) e final (Xf,Yf: usando rato) do elemento terminal
   %-----------------------------------------------------------------------------------
    figure(1)
    [Xf,Yf,button] = ginput(1);			%-- Seleccionar coordenada (X,Y) desejada ------
    if isempty(Xf) | isempty(Yf),		%-- Tecla ENTER para sair do ciclo infinito ----
       break
    end 
    
%for k=1:3,
      
   
%%%% ESPAÇO DAS JUNTAS ****************************************************************
   %----------------------------------------------------------------------------------*
   % Geração de trajectórias usando interpolação polinomial                           *
   %----------------------------------------------------------------------------------*
    if button == 1,                                                                  %*
       %------------------------------------------------------------------------------*
       % Cinemática Inversa: definição da configuração desejada                       *
       %------------------------------------------------------------------------------*
       [ang1f,ang2f] = CinInv2R(Xf,Yf,'b');	%-- Deslocamento angular final     *
       MinMovJuntas 											%-- Selecção do caminho mais curto *
       %------------------------------------------------------------------------------*
       % Planeamento de trajectórias no espaço das juntas                             *
       %------------------------------------------------------------------------------*
       [q1, dq1, ddq1] = GeradorTraj(t, [ang1i ang1f]);		%-- Junta 1 ("ombro") ----*
       [q2, dq2, ddq2] = GeradorTraj(t, [ang2i ang2f]);		%-- Junta 2 ("cotovelo") -*
       q = [q1; q2]; dq = [dq1; dq2]; ddq = [ddq1; ddq2];                            %*
    else                                                                             %*
        %-----------------------------------------------------------------------------*
        % 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];                                                    %*
        %-----------------------------------------------------------------------------*
        % 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] = CinInv2R(trajX,trajY,'b');			%-- Deslocamento angular ----------*
        [dq1,dq2,ddq1,ddq2] = CinInvDif2R(trajX,trajY,dtrajX,dtrajY,ddtrajX,ddtrajY);%*
        q = [q1; q2]; dq = [dq1; dq2]; ddq = [ddq1; ddq2];                           %*
    end                                                                              %* 
                                                                                     %* 
   %----------------------------------------------------------------------------------*
   % Dinâmica Inversa e PlotGrafs                                                     *
   %----------------------------------------------------------------------------------*
    [J C G T] = DinInv2R(q, dq, ddq);                                                %*
    PlotJuntasD			%-- Evolução temporal das posições, velocidades e acelerações *
    PlotBinD				%-- Evolução temporal dos binários      
    

%%%%*******************************************************************************END*







%%%% ESPAÇO DOS MÚSCULOS **************************************************************
   %----------------------------------------------------------------------------------*
   % Ombro: músculo flexor                                                            *
   %----------------------------------------------------------------------------------*
      z11 = sqrt(b(1,1)^2+R^2-2*b(1,1)*R*cos(q(1,:)));	%-- Dimensão   --------*
     dz11 = ( b(1,1)*R*sin(q0(1)+q(1,:)) ).*dq(1,:)./z11;		%-- Velocidade --------*
    ddz11 = b(1,1)*R*( (cos(q0(1)+q(1,:))-b(1,1)*R*(sin(q0(1)+q(1,:))./z11).^2).*... %*
               dq(1,:).^2+sin(q0(1)+q(1,:)).*ddq(1,:) )./z11;	%-- Aceleração --------*
      F11 = T(1,:).*z11./( b(1,1)*R*sin(q0(1)+q(1,:)) );			%-- Força--------------*

       
   %----------------------------------------------------------------------------------*
   % Ombro: músculo extensor                                                          *
   %----------------------------------------------------------------------------------*
      z12 = sqrt(b(1,2)^2+R^2-2*b(1,2)*R*cos(-q(1,:)));	%-- Dimensão   --------*
     dz12 = -( b(1,2)*R*sin(q0(2)-q(1,:)) ).*dq(1,:)./z12;		%-- Velocidade --------*
    ddz12 = -b(1,2)*R*((-cos(q0(2)-q(1,:))+b(1,2)*R*(sin(q0(2)-q(1,:))./z12).^2).*...%*
                dq(1,:).^2+sin(q0(2)-q(1,:)).*ddq(1,:) )./z12;	%-- Aceleração --------*
      F12 = T(1,:).*z12./( b(1,2)*R*sin(q0(2)-q(1,:)) );			%-- Força      --------*

   %----------------------------------------------------------------------------------*
   % Cotovelo: músculo flexor                                                         *
   %----------------------------------------------------------------------------------*
      z21 = sqrt(l1^2+b(2,1)^2+2*l1*b(2,1)*cos(q(2,:)));			%-- Dimensão   --------*
     dz21 = -( l1*b(2,1)*sin(q(2,:)) ).*dq(2,:)./z21;				%-- Velocidade --------*
    ddz21 = -l1*b(2,1)*( (cos(q(2,:))+l1*b(2,1)*(sin(q(2,:))./z21).^2).*...    		 %*
                dq(2,:).^2+sin(q(2,:)).*ddq(2,:) )./z21;			%-- Aceleração --------*
      F21 = T(2,:).*z21./( l1*b(2,1)*sin(q(2,:)) );				%-- Força      --------*


   %----------------------------------------------------------------------------------*
   % Cotovelo: músculo extensor                                                       *
   %----------------------------------------------------------------------------------*
      z22 = sqrt(l1^2+b(2,2)^2-2*l1*b(2,2)*cos(q(2,:)));			%-- Dimensão   --------*
     dz22 = ( l1*b(2,2)*sin(q(2,:)) ).*dq(2,:)./z22;				%-- Velocidade --------*
    ddz22 = l1*b(2,2)*( (cos(q(2,:))-l1*b(2,2)*(sin(q(2,:))./z22).^2).*...   		    %*
               dq(2,:).^2+sin(q(2,:)).*ddq(2,:) )./z22;			%-- Aceleração --------*
      F22 = T(2,:).*z22./( l1*b(2,2)*sin(q(2,:)) );				%-- Força      --------*

     
    %---------   -------------------------------------------------------------------------*
   % Evolução temporal das posições, velocidades e acelerações musculares             *
   %----------------------------------------------------------------------------------*
    PlotMusD                                                                         %*
   %----------------------------------------------------------------------------------*
   % Evolução temporal das forças nos músculos do ombro (F11,F12) e cotovelo (F21,F22)*
   %----------------------------------------------------------------------------------*
   PlotForD   
  % break
  

%%%%*******************************************************************************END*
disp('SIMULAÇÃO E CONTROLO: prima uma tecla'), pause




%%%% SIMULAÇÃO & CONTROLO *************************************************************
   %-----------------------------------------------------------------------------------
   % Simulação do movimento usando uma lei de controlo PD
   %-----------------------------------------------------------------------------------
    if button == 1, 
       Kp = 50.0;  Kd = 500.0;
    else
        Kp = 10.0;  Kd = 1000.0;
    end
    q1_r(1) = q1(1); q2_r(1) = q2(1); dq1_r(1) = dq1(1); dq2_r(1) = dq2(1);
    for k=1:DIM,
       %-- Controlador PD ------------------------------------------------------------
        err_q1 = q1(k)-q1_r(k);		err_dq1 = dq1(k)-dq1_r(k);
        err_q2 = q2(k)-q2_r(k);		err_dq2 = dq2(k)-dq2_r(k);
        
        T1(k) = Kp*err_q1+Kd*err_dq1;
        T2(k) = Kp*err_q2+Kd*err_dq2;
        %-- Dinâmica directa ----------------------------------------------------------
        if k <= DIM-1,
           z1 = q1_r(k); z2 = q2_r(k); z3 = dq1_r(k); z4 = dq2_r(k);
           [q1_r(k+1),q2_r(k+1),dq1_r(k+1),dq2_r(k+1)] = DirDin2R_RK4(z1,z2,z3,z4,T1(k),T2(k));
        end
    end
    q_r = [q1_r; q2_r]; dq_r = [dq1_r; dq2_r];
    T_r = [T1; T2];

   %-----------------------------------------------------------------------------------
   % Evolução temporal das variáveis no espaço das juntas (desejados e reais)
   %-----------------------------------------------------------------------------------
    PlotGrafR
    PlotBinariosR
    
   %---------------------------------------------------------------------------------------
   % Animação do movimento real do robô (controlo PD)
   %---------------------------------------------------------------------------------------  
    R = CinDir2R(q_r(1,:),q_r(2,:));
    DesenharRobo( P, q, q0, b ) 
       
   %---------------------------------------------------------------------------------------
   % Actualizar configuração inicial
   %---------------------------------------------------------------------------------------  
    LAST_R = R(:,DIM); LAST_q = q(:,DIM);
   
    clear R q dq
    Xi     = LAST_R(3);		Yi     = LAST_R(4);
    R(1,1) = LAST_R(1);		R(2,1) = LAST_R(2);
    R(3,1) = LAST_R(3);		R(4,1) = LAST_R(4);
%    q(1,1) = LAST_q(1);		q(2,1) = LAST_q(2);
    ang1i  = q_r(1,DIM);		ang2i  = q_r(2,DIM);

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