%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   INICIALIZAÇAO
clc;                        %
addpath Anexos;             %
porta=Inicializacao;        %
velocidade=4;               %
t=0.40;                      %
zeros=Calibracao;           %
homep=[ 85;                 % Tornozelo
        128;                % Joelho
        38];                % Anca
lado=0;                     %
tornozelo=homep(1)-0;       %
joelho=homep(2)-0;          %
anca=homep(3)-0;            %
%                           %
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % % SUBIR
[joelho,anca,tornozelo]=subida(joelho,anca,tornozelo,lado,porta,velocidade,zeros,t,homep);
      pause
% % % % INCLINAR A DIREITA
lado=-15;
     passo(joelho,anca,tornozelo,lado,porta,velocidade,zeros);
     pause
% % % INCLINAR A ESQUERDA
% lado=15;
%       passo(joelho,anca,tornozelo,lado,porta,velocidade,zeros);
%       pause
% % % % INCLINAR AO CENTRO
%  lado=0;
%       passo(joelho,anca,tornozelo,lado,porta,velocidade,zeros);
%       pause
% % % % DESCER
%  [joelho,anca,tornozelo]=descida(joelho,anca,tornozelo,lado,porta,velocidade,zeros,t,homep);
%       pause
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

     
     

% % % Mexer a Perna Esquerda (apoiado na direita)
DELTA_anca=0;
DELTA_joelho=0;
DELTA_tornozelo=0;
EXTRA_lado=50;

P_final=[lado                       % Pe direito ao lado        (+esq   -dir)   %
        tornozelo                   % Pe direito a frente       (-tras  +frente)%
        joelho;                     % Joelho direito            (+encolhe)      %
        lado                        %    Pe esquerdo ao lado    (+esq -dir)     %
        tornozelo+DELTA_tornozelo   %    Pe esquerdo a frente   (-tras  +frente)%
        joelho+DELTA_joelho;        %    Joelho esquerdo        (+encolhe)      %
        0                           % Anca direita virar        (+esq   -dir)   %
        lado-EXTRA_lado             % Anca direita lado         (+esq   -dir)   %
        anca;                       % Anca direita frente       (-tras  +frente)%
        0                           %    Anca esquerda virar    (+esq   -dir)   %
        lado                        %    Anca esquerda lado     (+esq   -dir)   %
        anca+DELTA_anca];           %    Anca esquerda frente   (-tras  +frente)%
enviar(porta,velocidade,zeros,P_final);

fclose(porta);