% Movimento das tres juntas de uma perna
% H   => Handle das comunicações
% leg => Posições das tres juntas da perna

function [error,errorstr]=motion(H,theta,speed)

% Inicialização dos parametros de saída
error=0;
errorstr='OK';

% Endereços dos SCUs
scu_id=[2 4];

% Dados da perna
footdata;

% Cálculo das posições a aplicar a cada SCU
pos=zeros(2,3);
pos(1,:)=theta;
pos(2,1)=-theta(2)-theta(3);
 
% Adaptação
for i=1:length(scu_id)
    % Conversão para os angulos dos servos
    [pos(i,:),error,errorstr]=convertangles(scu_id(i),pos(i,:));
    if (error==1), return, end
end

for i=1:length(scu_id)
    % Aplicar velocidade
    [x,error,errorstr]=applyjoint(H,scu_id(i),1,[speed speed speed]);
    if (error==1), return, end
    
    % Aplicar nova posição
    [x,error,errorstr]=applyjoint(H,scu_id(i),0,pos(i,:));
    if (error==1), return, end
end