% 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,leg,speed)

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

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

% Cálculo das posições a aplicar a cada SCU
pos=[     0  leg(1) leg(2);
     leg(3)      0       0];

% Adaptação
for i=1:length(scu_id)
    pos(i,:)=correct(scu_id(i),pos(i,:));
    pos(i,:)=adaptate(scu_id(i),pos(i,:));
    [pos(i,:),err,errstr]=validate(scu_id(i),pos(i,:));
    if (err==1)
        error=1;
        errorstr=errstr;
    end
end

% Se ocorreu erro de validação, sair da função sem aplicar o movimento
if (error==1), return, 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