% Controlador dos sensores de força
%
% theta=controller(theta,CoM)
%
% Entradas:
%   theta -> Posição actual das juntas
%   CoM   -> Centro de Massa
% Saídas:
%   theta -> Novas posições das juntas
%
% (c) Milton Ruas da Silva, 20/06/2006

function theta=controller_jacob(theta,CoM)

% Factores multiplicativos
CoM_factor=10;     % Factor multiplicativo do CoM
trig_factor=100;   % Os resultados das fc trig. são multip. por 100
r_factor=1;        % Distancia ao centro de massa em décimas de mm

% Parametros de compensação de cada junta
Kp=[-0.001; -0.001; 0.001];

% Inclusão das características da perna
legdata;

% Cálculo do Jacobiano transposto (aproximação a inversa)
phi=theta(1)*pi/180;
theta1=pi/2-theta(2)*pi/180;
theta2=theta(3)*pi/180;

l1=floor(eloL*r_factor);
l2=floor(eloL*r_factor);
r1=floor(l1/2*r_factor);
r2=floor(l2/2*r_factor);

% Factores trigonométricos
sin_phi=floor(sin(phi)*trig_factor);
cos_phi=floor(cos(phi)*trig_factor);
sin1=floor(sin(theta1)*trig_factor);
cos1=floor(cos(theta1)*trig_factor);
sin12=floor(sin(theta1+theta2)*trig_factor);
cos12=floor(cos(theta1+theta2)*trig_factor);

S12=((m1*r1+m2*l1)*sin1+m2*r2*sin12)/(m1+m2);
C12=((m1*r1+m2*l1)*cos1+m2*r2*cos12)/(m1+m2);
S2 = m2*r2*sin12/(m1+m2);
C2 = m2*r2*cos12/(m1+m2);

J =[ floor(S12*cos_phi/trig_factor) floor(C12*sin_phi/trig_factor) floor(C2*sin_phi/trig_factor);
     0                              -S12                           -S2                          ;
    -floor(S12*sin_phi/trig_factor) floor(C12*cos_phi/trig_factor) floor(C2*cos_phi/trig_factor)];
Jt=J';

% Adaptação do theta para o dos servos
servo=theta2servo(theta);

% Cálculo da nova posição
CoMref=[0; 0; 0];
error=floor((CoM-CoMref)*CoM_factor);
servo=servo+Kp.*(Jt*error);

% Limitação das posições
servo=limitservopos(servo);

% Reconversão para o angulo de trabalho do simulador
theta=servo2theta(servo);