%                   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 - 29-01-2007
%


%% Robo - Dinamica, Reacção, CoP, CoG %%

close all
clear all
clc
fprintf('\t\tRobo - Dinamica, Reacção, CoP, CoG\n');

scrsz = get(0,'ScreenSize');
figure('Position', [scrsz(3)/2 scrsz(4)/2 scrsz(3)/2 scrsz(4)/2], 'doublebuffer', 'on')


% inicialização de variaveis e constantes

    l1 = 1;                 % comprimento da tíbia
    l2 = 1;                 % comprimento do fémur
    r1 = l1/2;              % distancia da junta 1 ao centro de massa da tíbia
    r2 = l2/2;              % distancia da junta 2 ao centro de massa do fémur
    m1 = 1;                 % massa da tíbia
    m2 = 1;                 % massa do fémur
    g = 9.8;                % aceleração da gravidade
    t = 3;                  % tempo necessário para a execução do movimento
    f = 25;                 % numero de frames por segundo para animação do moviumento
    c = -1;                 % direcção do joelho (-1 -> lado direito; +1 -> lado esquerdo)
    angulo_tibia_x = pi/2;    % angulo inicial que a tíbia faz com a horizontal
    angulo_tibia_y = pi/2;
    angulo_femur = pi/2;    % angulo inicial que o fémur faz com a horizontal
    
    Go_back = 0;            % variavel sobre o tipo de demonstração, com retorno à posição inicial ou não    
        while(Go_back~='y' & Go_back~='n')
            Go_back = 'n';%input('Pretende que o robô volte sempre à posição inicial?(y,n)', 's');
        end
        if(Go_back=='y')    
            GB = 2;         % duplica o tempo de execução nos diferentes cálculos ao longo do script
        else
            GB = 1;
        end
        
    robo = zeros([3 3 GB*t*f]);                % Criação de vector de posição inicial das juntas (anca, joelho, pé)   
    robo(3,3,:) = ones(1,GB*t*f)*(2*l2/7);     % junta do pé

    L = [l1, l2];       %
    R = [r1, r2];       %
    M = [m1, m2];       %% matrizes auxiliares
    clc
    
% criação e animação do robo

    teta1 = pi/2-angulo_tibia_x;           % posição angular inicial da primeira junta
    teta2 = pi/2-angulo_tibia_y;           % posição angular inicial da segunda junta
    teta3 = angulo_femur - angulo_tibia_x;                % posição angular inicial da terceira junta
    pos_ref = [ones(1,GB*t*f)*robo(1,3); ones(1,GB*t*f)*robo(2,3); ones(1,GB*t*f)*robo(3,3)];     % posição da primeira junta, que serve de referencia para toda a estrutura móvel

    [robo(:,2,1), robo(:,1,1)] = direct_cine_3D(teta1, teta2, teta3, l1, l2);     % calcula no espaço cartesiano a posição das juntas de acordo com o ângulo especificado
    robo(:,1:2,1) = robo(:,1:2,1) + pos_ref(:,1:2);                 % normaliza a posição das juntas em relação à referencia -> primeira junta
    desenhar_2R_3D(robo(:,:,1), l1, l2);

    
    while(1),

        clc
        fprintf('\t\tRobo - Dinamica, Reacção, CoP, CoG\n');
        fprintf('L1 = %i;\tL2 = %i;\n', l1,l2);
        fprintf('M1 = %i;\tM2 = %i;\n', m1, m2);
        fprintf('Tempo de Execução -> %iseg\n', t*GB);

        
%         [a,b,c] = ginput(1);                % fica a aguardar que se indique a posição final a alcançar
%         while(sqrt((a-pos_ref(1,1))^2+(b-pos_ref(2,1))^2+(c-pos_ref(3,1))^2)>(l1+l2)),         % teste ao alcance do braço            
%                 disp('Posição fora da gama de alcance do braço');
%                 [a,b,c] = ginput(1);                
%         end
%          if(isempty([a,b]))     %
%              break;             %
%          end                    %% se for pressionado "enter" sai da simulação
%         a = -1;
%         b = 1/2;
%         c = 1;
         a = input('x = ')+robo(1,3);
         b = input('y = ')+robo(2,3);
         c = input('z = ')+robo(3,3);
        marca_3D(a, b, c, 'x');       %% indica na simulação a posição final a atingir   
         
         [teta1i, teta2i, teta3i] = inv_cine_3D(robo(1,1)-robo(1,3), robo(2,1)-robo(2,3), robo(3,1)-robo(3,3), l1, l2, c);  %
         [teta1f, teta2f, teta3f] = inv_cine_3D(a-robo(1,3), b-robo(2,3), c-robo(3,3), l1, l2, c);                  %
         

         [teta1, omega1, alfa1] = traject(teta1i, teta1f, t, f);              % calcula o trajecto para a 1ª junta
         [teta2, omega2, alfa2] = traject(teta2i, teta2f, t, f);              % calcula o trajecto para a 2ª junta
         [teta3, omega3, alfa3] = traject(teta3i, teta3f, t, f);              % calcula o trajecto para a 2ª junta

         if(Go_back=='y'),
             [teta1_f, omega1_f, alfa1_f] = traject(teta1f, teta1i, t, f);              % calcula o trajecto para a 1ª junta
             [teta2_f, omega2_f, alfa2_f] = traject(teta2f, teta2i, t, f);              % calcula o trajecto para a 2ª junta
             [teta3_f, omega3_f, alfa3_f] = traject(teta3f, teta3i, t, f);              % calcula o trajecto para a 2ª junta

             teta1 = [teta1, teta1_f];      %
             teta2 = [teta2, teta2_f];      %
             teta3 = [teta3, teta3_f];      %
                                            %
             omega1 = [omega1, omega1_f];   %
             omega2 = [omega2, omega2_f];   %
             omega3 = [omega3, omega3_f];   %
                                            %
             alfa1 = [alfa1, alfa1_f];      %
             alfa2 = [alfa2, alfa2_f];      %
             alfa3 = [alfa3, alfa3_f];      %% faz a duplicação dos vectores para acomodar a trajectória de retorno       
         end

         
         [mov2, mov1] = direct_cine_3D(teta1, teta2, teta3, l1, l2);  % converte o movimento para o espaço cartesiano

        
        TETA = [teta1; teta2; teta3];       %
        OMEGA = [omega1; omega2; omega3];   %
        ALFA = [alfa1; alfa2; alfa3];       %% matrizes auxiliares
        
        %[I, C, G, T] = DinInv3R(TETA, OMEGA, ALFA, M, L, R, g);     % cálculo do binário
%         Rea = RobGRF(TETA, OMEGA, ALFA, M, L, R, g);                % cálculo da força de reacção
%             Rt = Rea(1,:);                                          %% Reacção tangencial
%             Rn = Rea(2,:);                                          %% Reacção normal
%         CM = centro_massa_3R(TETA, M, L, R);            % cálculo do centro de massa
        %CP = T(1,:)./sqrt(Rt.^2+Rn.^2);                 % cálculo do centro de pressão -> CP = T/R            
        
        mov1 = mov1 + pos_ref;      %
        mov2 = mov2 + pos_ref;      %% normalização dos vectores movimento em relação à referencia - primeira junta
        robo(:,1,:) = mov1;         %
        robo(:,2,:) = mov2;         %% actualização da matriz robo para a posterior execução do movimento
        
        
        % construcção da matriz com as posições do rasto em cada instante
        rasto = ones([3, GB*t*f GB*t*f]);     
        i = 1;
        while(i<length(robo(1,2,:))),
            rasto(:,1:i,i) = robo(:,1,1:i);
            i = i+1;
        end
        
        % sequencia de animação
        i = 1;
        tic
        while(i<length(robo(1,1,:))),   
            desenhar_2R_3D(robo(:,:,i), l1, l2)                        %% desenha robô
            marca_3D(a, b, c, 'x');     %% marca a posição final
            hold on                                              
             plot3(rasto(1,1:i,i), rasto(2,1:i,i), rasto(3,1:i,i), 'k-.');               %% desenha rasto
                                                                        
             %plot3(CM(1,i), CM(2,i), CM(3,i), 'b+');                              %
             %plot3([CM(1,i) CM(1,i)], [CM(2,i) pos_ref(2,1,1)], 'b-.');  %
             %plot(CM(1,i), pos_ref(2,1,1), 'b*');                       %% centro de massa e respectiva projecção
                                                                        
             %plot3(CP(i), pos_ref(2,1,1), 'go');                         %% marca o centro de pressão
             title('simulador');                                                     
             hold off                                                   
            i = i+1;                                                    
            while(toc<1/f), end     %
            drawnow;                %% responsavel pela animação com f frames por segundo
            tic
        end
        
%         figure(gcf+1);      % cria uma janela de imagem consequente à anterior
%         subplot(3,1,1)
%          plot(0:1/f:(length(T(1,:))-1)/f, T(1,:), 'k');             %
%          title('Binário da primeira junta');                        %
%          xlabel('tempo(s)');                                        %
%          ylabel('Binário(N*m)');                                    % 
%          axis([0 length(T(1,:))/f min(T(1,:)) max(T(1,:))]);        %% plot do binário no braço 1 ao longo do tempo
%         subplot(3,1,2);                                                         
%          plot(0:1/f:(length(T(2,:))-1)/f, T(2,:), 'k');             %
%          title('Binário da segunda junta');                         %
%          xlabel('tempo(s)');                                        %
%          ylabel('Binário(N*m)');                                    %
%          axis([0 length(T(2,:))/f-1 min(T(2,:)) max(T(2,:))]);      %% plot do binário no braço 2 ao longo do tempo
%         subplot(3,1,3);                                                         
%          plot(0:1/f:(length(T(3,:))-1)/f, T(3,:), 'k');             %
%          title('Binário da terceira junta');                        %
%          xlabel('tempo(s)');                                        %
%          ylabel('Binário(N*m)');                                    %    
%          axis([0 length(T(3,:))/f-1 min(T(3,:)) max(T(3,:))]);      %% plot do binário no braço 2 ao longo do tempo
%         
%         figure(gcf+1);              % cria uma janela de imagem consequente à anterior
%         subplot(2,1,1)
%          plot(0:1/f:(length(Rt)-1)/f, Rt, 'k');                     %
%          title('Reacção tangencial do plano');                      %
%          xlabel('tempo(s)');                                        %
%          ylabel('Força(N)');                                        % 
%          axis([0 length(Rt)/f min(Rt) max(Rt)]);                    %% plot da força de reacção tangencial do plano ao longo do tempo
%         subplot(2,1,2);                                                        
%          plot(0:1/f:(length(Rn)-1)/f, Rn, 'k');                     %
%          title('Reacção normal do plano');                          %
%          xlabel('tempo(s)');                                        %
%          ylabel('Força(N)');                                        %        
%          axis([0 length(Rn)/f min(Rn) max(Rn)]);                    %% plot da força de reacção normal do plano ao longo do tempo
% 
%         figure(gcf-2);      % devolve a janela com o robô de simulação ao primeiro plano

        
        robo(:,:,1) = robo(:,:,end);
        
    end     % fim do ciclo infinito
        
    clc
    fprintf('fim de simulação\n');