function array=sample_traj3(H,T,pos0,posF,k)

scu=1;

% Caso deseje a reciclagem, activar esta flag!
recycle=1;

if (length(pos0)~=3 || length(posF)~=3 || length(k)~=3)
    error('sample_traj3:Invalid length','pos0, posF and k must have dimension 3');
end

% Adaptação das posições pedidas para os motores
%[pos0,x,e1]=geomtransf(1,pos0);
%[posF,x,e2]=geomtransf(1,posF);
%if (e1==1 || e2==1)
%    error('sample_traj3 function:invalid request','Cannot accomplish request');
%end

restPos=pos0;

zero=[0 0 0];
one =[1 1 1];
T_test=[T T T];
T_slow=[100 100 100];

% Duração do tick
tick=0.02;

% Controladores desligados
applycontrol(H,scu,3,zero);

% Deslocamento do servo para a posição inicial
applyjoint(H,scu,1,T_slow);
applyjoint(H,scu,0,pos0);

fprintf('Inicializando o servo...\n');

input('Pressione ENTER para ligar controlador... ');

% Controladores ligados
applycontrol(H,scu,0,[k(1,1) k(2,1) k(3,1)]);
applycontrol(H,scu,1,[k(1,2) k(2,2) k(3,2)]);
applycontrol(H,scu,2,[k(1,3) k(2,3) k(3,3)]);
applycontrol(H,scu,3,one);

fprintf('Controlador ligado!\n');

input('Pressione ENTER para iniciar movimento... ');
fprintf('Realizando Movimento...\n');

% Aplicação da velocidade e da posição final
applyjoint(H,scu,1,T_test);
applyjoint(H,scu,0,posF);

position=[];
velocity=[];
current =[];

i=1;
T2=0;
status=one;
while (sum(status(end-2:end))==3)
    [pos,status]=readjoint(H,scu,0);
end
while (sum(status(end-2:end))<3 || i<=T2)
    % Leitura dos parametros posição, velocidade e corrente
    [pos,status]=readjoint(H,scu,0);
    vel =readjoint(H,scu,1);
    curr=readjoint(H,scu,2);
    
    % Armazenamento dos dados dos sensores
    position=[position; pos ];
    velocity=[velocity; vel ];
    current =[current ; curr];
    
    if (T2==0 && sum(status(end-2:end))==3)
        T1=i;
        T2=floor(T1*3.0);
    end
    i=i+1;
end

input('Pressione ENTER para reciclar o servo...');
applycontrol(H,scu,3,zero);
if (recycle~=0)
    applyjoint(H,scu,1,T_slow);
    applyjoint(H,scu,0,restPos);
end

fprintf('Efectuando alguns cálculos...\n');
% Gama temporal
step=T/T1;
time=[step:step:T2*step]'*tick;
t1=T*tick;
t2=time(end);

% Coeficientes do polinómio descritor da trajectória
delta=posF-pos0;                                % Variação angular
a=[ pos0; zero; 3*delta/t1^2; -2*delta/t1^3];

% Equações da posição e velocidade
syms t;
posEqu=a(1,:)+a(2,:)*t+a(3,:)*t^2+a(4,:)*t^3;
velEqu=diff(posEqu,t);

% Cálculo dos valores esperados
posT=[ones(T2,1).*subs(posEqu(1),time) ones(T2,1).*subs(posEqu(2),time) ones(T2,1).*subs(posEqu(3),time)];
velT=[ones(T2,1).*subs(velEqu(1),time) ones(T2,1).*subs(velEqu(2),time) ones(T2,1).*subs(velEqu(3),time)];
posT(T1+1:end,1)=posF(1);   posT(T1+1:end,2)=posF(2);   posT(T1+1:end,3)=posF(3);
velT(T1+1:end,1)=0;         velT(T1+1:end,2)=0;         velT(T1+1:end,3)=0;

% Número de posições armazenadas para estimação da velocidade
N_POS=5;
% Factor de normalização da corrente medida
N_CURR=52;

h1=figure;
h2=figure;
for i=1:3
    figure(h1);
    
    subplot(3,2,i*2-1);
    plot(time,posT(:,i),time,position(:,i));
    grid on;
    
    subplot(3,2,i*2);
    plot(time,posT(:,i)-position(:,i));
    grid on;
    
    figure(h2);
    
    subplot(3,2,i*2-1);
    plot(time,velT(:,i),time,velocity(:,i)/(tick*N_POS));
    grid on
    
    subplot(3,2,i*2);
    plot(time,current(:,i)/N_CURR);
    axis([0 t2 0 1]);
    grid on
end

array=[time position posT velocity velT current];
save traject.txt array -ASCII -TABS

fprintf('Finished!\n\n');