clc
clear all
close all

% Intervalo de tempo da simulação
t=[0:0.02:10];

% Período de movimento do plano de apoio
T=5;

% Máxima inclinação de cada eixo do plano de apoio
max_gama_x=30;
max_gama_y=30;

% Gama de variação de cada eixo do plano de apoio
gama_x=max_gama_x*sin(2*pi/T*t);
gama_y=max_gama_y*sin(2*pi/T*t);

% Posições iniciais das juntas (orientação vertical)
theta=[0; 0; 0];

figure(1);
set(1,'name','Perna 3D','doublebuffer','on','backingstore','off')

for i=1:length(t)
    gama=[gama_x(i) gama_y(i)];
    
    subplot(1,2,1)
    CoM=calccom(theta,gama);
    fall=drawleg(theta,gama,CoM);
    
    subplot(1,2,2)
    drawfoot(CoM(:,3));
    
    if (fall==1)
        text(0,0,'GAME OVER')
        disp 'PERNA EM QUEDA!'
        break;
    end
    
    % Desenhar agora
    drawnow
    
    % Na primeira iteracção fazer pausa
    if (i==1), pause, end
    
    % Controlador
    %theta=controller(theta,CoM(:,3));
    theta=controller_jacob(theta,CoM(:,3));
end

disp 'Program finished!'