%                   Universidade de Aveiro
%                               -
%       Departamento de Electrónica e Telecomunicações
%
% Projecto 5ºAno - Desenvolviento de Algoritmos de Controle para um Robô
% Humanoide
%
% Pedro Miguel Ferreira     
% Nº Mec - 27593
%
% Data - 07-03-2007
%
%%DESENHAR_2R_3D   Faz o desenho do robô com 1 perna - Caso particular
%   desenhar_2R_3D(x, l1, l2, l3, handle, color)
%   PARAMETROS DE ENTRADA:
%       x é a matriz com a posição cartesiana das 2 juntas
%       l1 é o comprimento do 1º segmento
%       l2 é o comprimento do 2º segmento
%       l3 é o comprimento do 3º segmento
%       handle da figura
%       color é a cor do robô na simulação

function desenhar_3R_2D(x, l1, l2, l3, handle, color)


figure(handle);

DP = x(2,end);  % distancia da base do pé à primeira junta
HP = 2;         % altura do pé
CP = 10;        % metade do comprimento do pé

% criação do pé
    x(:,end+1) = [x(1,end); x(2,end)-DP+HP];        % espaço entre o pé e a junta
    x(:,end+1) = [x(1,end)-CP; x(2,end)];           %    canto superior esquerdo
    x(:,end+1) = [x(1,end)+2*CP; x(2,end)];         %    canto superior direito
    x(:,end+1) = [x(1,end); x(2,end)-HP];           %    canto inferior direito
    x(:,end+1) = [x(1,end)-2*CP; x(2,end)];         %    canto inferior esquerdo
    x(:,end+1) = [x(1,end); x(2,end)+HP];           %% plataforma do pé

% plots
    plot(x(1,:), x(2,:), color); hold on            %
    plot(x(1,1:end-6), x(2,1:end-6), 'bo');         % faz o plot do robo

    maxi = l1+l2+l3+(l1+l2+l3)/3;
    plot([-maxi+x(1,4) maxi+x(1,4)], [0 0], 'k');   % cria a linha de superficie

% definições dos eixos
    axis equal
    axis([-maxi+x(1,4) maxi+x(1,4) -maxi+x(2,4) maxi+x(2,4)]);  % estabelece os eixos
    hold off

drawnow;