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

function [CoPstore,errorstore,deltastore,servostore]=sim2(CoPref,Nsamples,Nshow)

% Preenchimento dos argumentos
if (nargin<3)
    Nshow=10;
end
if (nargin<2)
    Nsamples=1000;
end
if (nargin==0)
    CoPref=[0 0 0];
end

%CoPref(3)=-CoPref(3);

close all

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

% Massa em Dg dos elos
M1=45;
M2=45;

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

% Comprimento dos elos (mm)
L0=20;
L1=95;
L2=95;

% 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=[26 26 26];

% 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*10)./servo_nratio;
    theta(2)=theta(2)+90;
   
    % Cálculo do Centro de Pressão
    phi=theta*pi/180;
    CoMvert=((M1*R1+M2*L1)*sin(phi(2))+M2*R2*sin(phi(2)+phi(3)))/(M1+M2);
    CoMhorz=((M1*R1+M2*L1)*cos(phi(2))+M2*R2*cos(phi(2)+phi(3)))/(M1+M2);
    h=L0+L1+L2-(L0+L1*sin(phi(2))+L2*sin(phi(2)+phi(3)))*cos(phi(1));
    
    CoP=[CoMvert*sin(phi(1)), CoMhorz, h];
    CoPstore=[CoPstore; CoP];
    
    % Cálculo da matriz Jacobiana
    s12=((M1*R1+M2*L1)*seno(theta(2))+M2*R2*seno(theta(2)+theta(3)))/(M1+M2);
    c12=((M1*R1+M2*L1)*coseno(theta(2))+M2*R2*coseno(theta(2)+theta(3)))/(M1+M2);
    s2=M2*R2*seno(theta(2)+theta(3))/(M1+M2);
    c2=M2*R2*coseno(theta(2)+theta(3))/(M1+M2);
   
%     Jt=[s12*coseno(theta(1)),         0, -s12*seno(theta(1))  ;
%         c12*seno(theta(1)),   -s12*1000,  c12*coseno(theta(1));
%          c2*seno(theta(1)),    -s2*1000,   c2*coseno(theta(1))]/(10000);

    J=[s12*coseno(theta(1)),   c12*seno(theta(1)),   c2*seno(theta(1)),
                          0,            -s12*1000,            -s2*1000,
        -s12*seno(theta(1)), c12*coseno(theta(1)), c2*coseno(theta(1))]/(10000);

    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')/1e4;
    delta=servo_signal'.*K.*(Jt*error')/1e4;
    delta=delta';
    %delta=delta'.*servo_signal;
    
    deltastore=[deltastore; delta];
    
    % Actualização da posição
    servo=servo+delta;
    
    % Limitação da posição dos servos
    servo=(abs(servo)>[90 90 90]).*90.*sign(servo)+(abs(servo)<=[90 90 90]).*servo;
    
    if (mod(i,Nshow)==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

% ____ Subfunction seno ________
function res=seno(arg)
res=round(sin(arg*pi/180)*1000);

% ____ Subfunction coseno ______
function res=coseno(arg)
res=round(cos(arg*pi/180)*1000);
