%--------------------------------------------------------------------------------------
% 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 e OPÇOES
%--------------------------------------------------------------------------------------
 go=9.8;					%-- Aceleração da gravidade -------------------------------
%--------------------------------------------------------------------------------------
%-- MODOS DE SIMULAÇAO (espaço Cartesiano)
%--  (1) Movimento com transitorio: arranque&paragem  -> ModLocom = 'Transitorios'
%--  (2) Movimento estacionario: velocidade constante -> ModLocom = 'Estacionario'
%--------------------------------------------------------------------------------------
 ModLocom = 'Estacionario';
%--------------------------------------------------------------------------------------
%-- GERAÇAO/OPTIMIZAÇAO DO MOVIMENTO DO TRONCO
%--  OptimizarMovTronco = 0 -> gera a postura/trajectoria do tronco
%--  OptimizarMovTronco = 1 -> optimizaçao "estatica" do movimento do tronco
%--------------------------------------------------------------------------------------
 COPd = 0;               %-- COP ou ZMP desejado no processo de optimizaçao
 OptimizarMovTronco = 1; %-- Geraçao(0)/Optimizaçao(1) da trajectorias do tronco   *

%-- 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.21;	%-- Comprimento e massa do elo anca-anca --------------
l5=0.175;    r5=l5;   m5=5.5;	%-- Comprimento e massa da carga colocada no tronco ---
L = [l0 l1 l2 l3 l4 l5];   M = [m0 m1 m2 m3 m4 m5];
%-- Simulação temporal ----------------------------------------------------------------
Vf      = 0.1;          %-- Velocidade de progressao       ----------------------------
Ts		= 20.0e-3;		%-- Período de amostragem          ----------------------------
h		= Ts;				%-- Período de simulação           ------------------------

%--------------------------------------------------------------------------------------
% 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
%--------------------------------------------------------------------------------------
q1i=0; q2i=0; q3i=0; q4i=0; q5i=0;
qi = [q1i q2i q3i q4i q5i];
P = CinDir5dof(q1i,q2i,q3i,q4i,q5i,l0,l1,l2,l3,l4,l5);
[C1,C2,C3,C4,C5] = CoordJuntas( q1i,q2i,q3i,q4i,q5i,l0,l1,l2,l3,l4,l5 );
COG = RobCOG( qi,l0,l1,l2,l3,l4,l5,r0,r1,r2,r3,r4,r5,m0,m1,m2,m3,m4,m5 );
DesenharRoboPZ( P, C1, C2, C3, C4, C5, COG )

%--------------------------------------------------------------------------------------
% Modificar ponto de vista: VIEW
%--------------------------------------------------------------------------------------
[AZ,EL] = view;
%[AZ,EL] = ModificarView(AZ,EL);

%--------------------------------------------------------------------------------------
% Animaçao do movimento variando juntas individualmente 
%--------------------------------------------------------------------------------------
disp('-- Press a key to begin HOMING ------------------------------------------------')
pause, Homing

%--------------------------------------------------------------------------------------
% Ciclo infinito: descrição dos requisitos da tarefa operacional
%--------------------------------------------------------------------------------------
clc
disp('-- Press a key to begin PtP Motion --------------------------------------------')
disp(' Moving down !!! '), pause
Xhi = P(13); Yhi = P(14); Zhi = P(15);      %-- From ...
Xhf = 0.0;   Yhf = 0.0;   Zhf = 0.1;        %-- ... to  
if ( Xhf^2+(Zhf-l0-l1)^2 > (l2+l3)^2 ), disp('Out of Space ...'), pause, end
[q1f,q2f,q3f,q4f,q5f] = CinInv5dof(Xhf,Yhf,Zhf,l0,l1,l2,l3,l4,l5);
qf = [q1f q2f q3f q4f q5f];
MovePtP(qi,qf,l0,l1,l2,l3,l4,l5,r0,r1,r2,r3,r4,r5,m0,m1,m2,m3,m4,m5)

%--------------------------------------------------------------------------------------
clc
disp('-- Press a key to begin PtP Motion --------------------------------------------')
disp(' Moving up/left !!! '), pause
Xhi = Xhf; Yhi = Yhf; Zhi = Zhf;
Xhf = -0.1;   Yhf = 0.05;   Zhf = 0.25;
if ( Xhf^2+(Zhf-l0-l1)^2 > (l2+l3)^2 ), disp('Out of Space ...'), pause, end
[q1f,q2f,q3f,q4f,q5f] = CinInv5dof(Xhf,Yhf,Zhf,l0,l1,l2,l3,l4,l5);
qi = qf; qf = [q1f q2f q3f q4f q5f];
MovePtP(qi,qf,l0,l1,l2,l3,l4,l5,r0,r1,r2,r3,r4,r5,m0,m1,m2,m3,m4,m5)

%--------------------------------------------------------------------------------------
clc
disp('-- Press a key to begin PtP Motion --------------------------------------------')
disp(' Moving forward ... '), pause
Xhi = Xhf; Yhi = Yhf; Zhi = Zhf;
Xhf = 0.1;   Yhf = -0.05;   Zhf = 0.25;
if ( Xhf^2+(Zhf-l0-l1)^2 > (l2+l3)^2 ), disp('Out of Space ...'), pause, end
[q1f,q2f,q3f,q4f,q5f] = CinInv5dof(Xhf,Yhf,Zhf,l0,l1,l2,l3,l4,l5);
qi = qf; qf = [q1f q2f q3f q4f q5f];
MovePtP(qi,qf,l0,l1,l2,l3,l4,l5,r0,r1,r2,r3,r4,r5,m0,m1,m2,m3,m4,m5)

%--------------------------------------------------------------------------------------
% Sair
%--------------------------------------------------------------------------------------
 disp('Press any key to exit'), pause, close all, return
