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

restPos=pos0;

scu=1;

% % Extremos do movimento
% pos0=+70;
% posF=-70;
% % Duração do movimento
% T=100;
% %_________________________

if (nargin==4)
    k=[0 0 0];
end

T_slow=100;
% Duração do tick
tick=0.02;

% Número de posições armazendas para estimação da velocidade
N_POS=5;

% Controladores desligados
applycontrol(H,scu,3,[0 0 0]);

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

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

% status=1;
% while (status(end)==1)
%     [pos,status]=readjoint(H,scu,0);
% end
% while (status(end)==0) 
%     % Leitura dos parametros posição, velocidade e corrente
%     [pos,status]=readjoint(H,scu,0);
% end

input('Pressione ENTER para ligar controlador... ');
% Controladores ligados
if (sum(k)~=0)
    applycontrol(H,scu,0,[k(1) 0 0]);
    applycontrol(H,scu,1,[k(2) 0 0]);
    applycontrol(H,scu,2,[k(3) 0 0]);
    applycontrol(H,scu,3,[1 0 0]);
end
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 0 0]);
applyjoint(H,scu,0,[posF 0 0]);

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

i=1;
T2=0;
status=1;
while (status(end)==1)
    [pos,status]=readjoint(H,scu,0);
end
while (status(end)==0 || 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 && status(end)==1)
        T1=i;
        T2=floor(T1*3.0);
    end
    i=i+1;
end

input('Pressione ENTER para reciclar o servo...');
applycontrol(H,scu,3,[0 0 0]);
applyjoint(H,scu,1,[T_slow 0 0]);
applyjoint(H,scu,0,[restPos 0 0]);

input('Pressione ENTER para terminar...');
applyjoint(H,scu,1,[0 0 0]);

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, 0, 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=subs(posEqu,time);
velT=subs(velEqu,time);
posT(T1+1:end)=posF;
velT(T1+1:end)=0;

close all
figure;
plot(time,current(:,1));
%figure;
%plot(time,velocity(:,1)/(tick*N_POS),time,velT);
figure;
plot(time,posT-position(:,1));
figure;
plot(time,position(:,1),time,posT);

array=[time position(:,1) velocity(:,1) current(:,1) posT velT];
%save traject=-70+70#T=200#M=m0#t:pos:vel:curr:posTh:velTh.txt array -ASCII -TABS

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