function [t,servo,reaction]=poly5(H,scu,pos,rounds,speed,Kone,filename)

if (nargin<6)
    Kone=30;
end
if (nargin<5)
    speed=100;
end
if (nargin<4)
    rounds=1;
end

% Ganhos de compensação
K=[Kone(1) Kone(1) Kone(1)];
K2=[Kone(2) Kone(2) Kone(2)];

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

scu2=scu+2;
scu_all=[scu scu2];

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

% Desligar compensação
[x,err,errstr]=applycontrol(H,scu,3,[0 0 0]);
if (err==1), return, end
[x,err,errstr]=applycontrol(H,scu2,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
[x,err,errstr]=applyjoint(H,scu2,1,[100 100 100]);
if (err==1), return, end

% Activar PWMs
[err,errstr]=inituc(H,scu_all);
if (err==1), return, end
% Esperar que a activação dos PWMs seja feito
disp 'Activando PWMs...'
[stat,error,errorstr]=waitpwmon(H,scu_all);
if (stat==0), return, end
disp 'PWMs activados!'

% ____ INICIALIZAÇÃO DOS SENSORES DE FORÇA ___________________________

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

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

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

% ____ INICIALIZAÇÃO DOS INCLINÓMETROS _______________________________

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

% ____ ACTIVAÇÃO DOS CONTROLADORES ___________________________________

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

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

disp 'Pressione ENTER para iniciar...'
pause

tic
% Enquanto os motores estiverem em movimento, medir os sensores
round=0;
pos_index=1;
delay=[];

t_border=[];

while (1)

    % 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 da posição
    [srv2,x,x,err,errstr]=readjoint(H,scu2,0);
    if (err==1), return, end

    % Leitura dos sensores de força
    [react,x,err,errstr]=readspecial(H,scu);
    if (err==1), return, end
    
    % Leitura dos sensores de força
    [react2,x,err,errstr]=readspecial(H,scu2);
    if (err==1), return, end

    t=[t; t_instant];
    servo=[servo; srv];
    reaction=[reaction; react];
    servo2=[servo2; srv2];
    reaction2=[reaction2; react2];
    
    % Terminou o movimento
    [stat,x,x,x,err,errstr]=statmotionfin(H,scu_all);
    if (err==1), return, end
    if (stat==1)
        % Actualização do número de voltas
        if (pos_index==1), round=round+1, end

        if (round>rounds), break, end
        
        pos_index=mod(pos_index,size(pos,1))+1;

        %pause(4);
        
        % Actualização do Centro de Pressão referencia
        [x,err,errstr]=applycontrol(H,scu,1,pos(pos_index,:));
        if (err==1), return, end

        % Esperar que os motores arranquem
        [stat,err,errstr]=motionstart(H,scu_all);
        if (stat==0 || err==1), return, end
        
        t_border=[t_border; toc];
    end
end

disp 'Finished!'
pause(3);

% Desligar compensação
[x,err,errstr]=applycontrol(H,scu,3,[0 0 0]);
if (err==1), return, end
[x,err,errstr]=applycontrol(H,scu2,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_all);
if (err==1), return, end

% Salvaguarda do workspace
save(filename,'scu_all','speed','pos','K','K2','t','servo','reaction','servo2','reaction2','rounds','t_border')