function [err,errstr]=estatica(H,scu,pos0,posF,steps)

% Ganhos de compensação do controlador Jacobiano
K=[50 50 50];

speed=50;

% Dados do pé
footdata;

% Desactivar PWMs
[err,errstr]=killuc(H,[scu scu+2]);
if (err==1), return, end

% Desligar compensação
[x,err,errstr]=applycontrol(H,scu,3,[0 0 0]);
if (err==1), return, end

% Inicializar velocidade dos servos
[x,err,errstr]=applyjoint(H,scu,1,[100 100 100]);
if (err==1), return, end

% Activar PWMs
[err,errstr]=inituc(H,[scu scu+2]);
if (err==1), return, end
% Esperar que a activação dos PWMs seja feito
disp 'Activando PWMs...'
pause(4)
disp 'PWMs activados!'

% Inicializar ganhos de compensação
[x,err,errstr]=applycontrol(H,scu,0,K);
if (err==1), return, end

% Velocidade da variação do centro de pressão
[x,err,errstr]=applycontrol(H,scu,2,[speed speed speed]);
if (err==1), return, end

% Actualizar centro de Pressão referencia
[x,err,errstr]=applycontrol(H,scu,1,pos0);
if (err==1), return, end

% Activar compensador
[x,err,errstr]=applycontrol(H,scu,3,[3 3 3]);
if (err==1), return, end

disp 'Pressione ENTER para iniciar percurso...'
pause

N=steps+1;
delta=(posF-pos0)/steps;

pos_list=zeros(N,3);
for i=1:length(pos0)
    if (delta(i)==0)
        pos_list(:,i)=ones(N,1)*pos0(i);
    else
        pos_list(:,i)=[pos0(i):delta(i):posF(i)]';
    end
end

servo=zeros(N,3);
reaction=zeros(N,4);

for i=1:N
    
    % Posição a aplicar
    pos=pos_list(i,:)
    
    % Actualizar centro de Pressão referencia
    [x,err,errstr]=applycontrol(H,scu,1,pos);
    if (err==1), return, end

    % Desenhar pé com o CoP referência actual
    drawfoot(pos);
    
    fprintf('Posição=[%g,%g,%g]\n',pos);
    
    % Esperar que o movimento termine
    [x,err,errstr]=waitmotionfin(H,scu);
    if (err==1), return, end
    
    % Esperar 1 segundo
    pause(2);
    
    % Leitura dos sensores de força
    [react,x,err,errstr]=readspecial(H,scu);
    if (err==1), return, end
    % Leitura da posição
    [srv,x,x,err,errstr]=readjoint(H,scu,0);
    if (err==1), return, end
    
    % Armazenar valores
    reaction(i,:)=react;
    servo(i,:)=srv;
end

% Centro de Pressão referencia nulo
[x,err,errstr]=applycontrol(H,scu,1,[0 0 0]);
if (err==1), return, end

% Esperar 2 segundos
pause(speed/50*2);

% Desligar compensação
[x,err,errstr]=applycontrol(H,scu,3,[0 0 0]);
if (err==1), return, end

pause(2);

% Desactivar PWMs
[err,errstr]=killuc(H,[scu scu+2]);
if (err==1), return, end

save cop_static_xx.mat scu pos0 posF steps speed delta pos_list servo reaction