function [err,errstr]=elipse(H,X,Y,Nsamples,Nrounds,delay)

if (nargin<6)
    delay=0.1;
end
if (nargin<5)
    Nrounds=1;
end
if (nargin<4)
    Nsamples=50;
end
if (nargin<3)
    error('You need at least 3 arguments!');
end

scu=2;

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

% 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

[x,err,errstr]=applyjoint(H,scu,1,[50 50 50]);
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,1,K);
if (err==1), return, end

theta=0;
x=X*sin(theta);
y=Y*cos(theta);

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

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

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

% Número total de amostras
N=Nsamples*Nrounds;
delta_theta=2*pi/Nsamples;

% Leitura da posição inicial
[servo0,x,x,err,errstr]=readjoint(H,scu,0);
if (err==1), return, end

servo0

% Arrays de armazenamento
t_store=zeros(N,1);
theta_store=zeros(N,1);
trajxy_store=zeros(N,2);
servo_store=zeros(N,3);
reaction_store=zeros(N,4);

tic
for i=1:N
    
    % Actualização do theta
    theta=mod(delta_theta*i,2*pi);
    theta_store(i)=theta;
    
    % Cálculo das coordenadas do centro de pressão
    x=X*sin(theta);
    y=Y*cos(theta);
    trajxy_store(i,:)=[x y];

    t_store(i)=toc;
    
    % Centro de Pressão referencia inicial
    [x,err,errstr]=applycontrol(H,2,2,[x y 0]);
    if (err==1), return, end

    % Leitura da posição
    [servo_store(i,:),x,x,err,errstr]=readjoint(H,scu,0);
    if (err==1), return, end
    
    % Leitura dos sensores de força
    [reaction_store(i,:),x,err,errstr]=readspecial(H,scu);
    if (err==1), return, end

    pause(delay);
end

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

pause(3);

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

% Salvaguarda do workspace
save elipse.mat X Y delay Nsamples Nrounds N delta_theta K t_store theta_store trajxy_store reaction_store servo_store servo0

plot(t_store,trajxy_store,'.',t_store,reaction_store);
