% 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(theta,CoM)

% Parametros de compensação de cada junta
Kp1=10;
Kp2=10;

% Componentes do centro de massa projectados sobre o plano do pé
CoMx=CoM(1);
CoMy=CoM(2);

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

% Cálculo da nova posição
servo(1)=servo(1)-Kp1*CoMx;
servo(2)=servo(2)+Kp2*CoMy;

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

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