% Simula a resposta do controlador de equilíbrio dado um centro de pressão
% referencia CoPref, para as primeiras Nsamples amostras.

function sim(CoPref,Nsamples)

close all

% Ganho do compensador
K=[0.5; 0.5; -0.5];

% Massa em Kg
M1=0.45;
M2=0.45;

% Distancia ao centro de massa de cada elo (mm)
R1=4.7;
R2=4.7;

% Comprimento dos elos (mm)
L0=2.0;
L1=9.5;
L2=9.5;

% Características dos motores relativamente aos angulos das juntas
servo=[0 10 50];
servo_offset=[0 0 70];
servo_signal=[+1 +1 -1];
servo_nratio=[2.6 2.6 2.6];

% Variáveis de armazenamento
CoPstore=[];                    % Centro de Pressão medido
errorstore=[];                  % Erro do Centro de Pressão
deltastore=[];                  % Incremento de posição
servostore=[];                  % Angulos dos servos

for i=1:Nsamples
    
    % Armazenar angulos dos motores
    servostore=[servostore; servo];
    
    % Clacular angulos das juntas
    theta=((servo-servo_offset).*servo_signal)./servo_nratio;
    theta(2)=theta(2)+90;
    %theta=servo;
    theta=theta*pi/180;
   
    % Cálculo do Centro de Pressão
    CoMvert=((M1*R1+M2*L1)*sin(theta(2))+M2*R2*sin(theta(2)+theta(3)))/(M1+M2);
    CoMhorz=((M1*R1+M2*L1)*cos(theta(2))+M2*R2*cos(theta(2)+theta(3)))/(M1+M2);
    h=L0+L1+L2-(L0+L1*sin(theta(2))+L2*sin(theta(2)+theta(3)))*cos(theta(1));
    
    %CoP=[CoMvert*sin(theta(1)), CoMhorz, CoMvert*cos(theta(1))];
    CoP=[CoMvert*sin(theta(1)), CoMhorz, h];
    CoPstore=[CoPstore; CoP];
    
    % Cálculo da matriz Jacobiana
    s12=((M1*R1+M2*L1)*sin(theta(2))+M2*R2*sin(theta(2)+theta(3)))/(M1+M2);
    c12=((M1*R1+M2*L1)*cos(theta(2))+M2*R2*cos(theta(2)+theta(3)))/(M1+M2);
    s2=M2*R2*sin(theta(2)+theta(3))/(M1+M2);
    c2=M2*R2*cos(theta(2)+theta(3))/(M1+M2);
    
%     Jt=[s12*cos(theta(1)),    0, -s12*sin(theta(1));
%         c12*sin(theta(1)), -s12,  c12*cos(theta(1));
%          c2*sin(theta(1)),  -s2,   c2*cos(theta(1))];

    J=[ s12*cos(theta(1)), c12*sin(theta(1)), c2*sin(theta(1));
                        0,              -s12,              -s2;
       -s12*sin(theta(1)), c12*cos(theta(1)), c2*cos(theta(1))];
        
    Jt=J';

    % Cálculo do Erro
    error=CoPref-CoP;
    %error(3)=0;
    error(3)=-error(3);
    errorstore=[errorstore; error];
    
    % Cálculo do deslocamento a aplicar
    delta=K.*(Jt*error');
    delta=delta';
    %delta=delta'.*servo_signal;
    
    deltastore=[deltastore; delta];
    
    % Actualização da posição
    servo=servo+delta;
    
    if (mod(i,10)==0)
        subplot(2,2,1)
        plot([1:i]',CoPstore,'linewidth',2);
        title('Centre of Pressure');
        grid on
        subplot(2,2,2)
        plot([1:i]',deltastore,'linewidth',2);
        title('Position increment');
        grid on
        subplot(2,2,3)
        plot([1:i]',errorstore,'linewidth',2);
        title('Error');
        grid on
        subplot(2,2,4)
        plot([1:i]',servostore,'linewidth',2);
        title('Servo positions');
        grid on
    
        drawnow
    end
end