function [t,servo,reaction]=poly5(H,scu,pos0,posF,speed,Kone)

% Ganhos de compensação
K=[Kone Kone Kone];

% Arrays de armazenamento
t=[];
servo=[];
reaction=[];

% 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

% Centro de Pressão referencia inicial
[x,err,errstr]=applycontrol(H,scu,1,pos0);
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

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

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

% Aplicação do centro de Pressão referencia final
[x,err,errstr]=applycontrol(H,scu,1,posF);
if (err==1), return, end

% Esperar que os motores arranquem
stat=0;
while (stat==0)
    [stat,err,errstr]=motionstart(H,scu);
    if (err==1), return, end
end

tic
% Enquanto os motores estiverem em movimento, medir os sensores
stat=0;
while (stat<2)

    % Instante de tempo
    t_instant=toc;
    
    % Leitura da posição
    [srv,x,x,err,errstr]=readjoint(H,scu,0);
    if (err==1), return, end
    
    % Leitura dos sensores de força
    [react,x,err,errstr]=readspecial(H,scu);
    if (err==1), return, end

    t=[t; t_instant];
    servo=[servo; srv];
    reaction=[reaction; react];
    
    if (stat==0)
        % Verificar se os motores terminaram o movimento
        [stat,x,x,err,errstr]=statmotionfin(H,scu);
        if (err==1), return, end
        if (stat==1), t0=t_instant; end
    elseif (t_instant-t0>speed/50)
        stat=2;
    end
end

disp 'Finished!'
pause(3);

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

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

% Esperar
pause(3);

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

% Salvaguarda do workspace
save polyc.mat scu speed pos0 posF K t servo reaction
