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

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

% Ganhos de compensação
Ki=[1 1 1]*Kall(1);
Kp=[1 1 1]*Kall(2);
K =[1 1 1]*Kall(3);

% 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...'
[stat,error,errorstr]=waitpwmon(H,scu);
if (stat==0), return, end
disp 'PWMs activados!'

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

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

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

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

% Inicialzar ganhos do controlador local
[x,err,errstr]=applycontrol(H,scu,0,Ki);
if (err==1), return, end
[x,err,errstr]=applycontrol(H,scu,1,Kp);
if (err==1), return, end

% Inicializar ganhos de compensação (activação do controlador)
[x,err,errstr]=applycontrol(H,scu,2,K);
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 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];
    
    % Terminou o movimento
    [stat,x,x,x,err,errstr]=statmotionfin(H,scu);
    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]=applyjoint(H,scu,0,pos(pos_index,:));
        if (err==1), return, end

        % Esperar que os motores arranquem
        [stat,err,errstr]=motionstart(H,scu);
        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,2,[0 0 0]);
if (err==1), return, end
[x,err,errstr]=applycontrol(H,scu,1,[0 0 0]);
if (err==1), return, end
[x,err,errstr]=applycontrol(H,scu,0,[0 0 0]);
if (err==1), return, end

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

% Mudar controlo para malha aberta
[x,err,errstr]=applycontrol(H,scu,3,[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(filename,'scu','speed','pos','Ki','Kp','K','t','servo','reaction','rounds','t_border')
